/*
 * This file is part of the OpenMV project.
 *
 * Copyright (c) 2013-2021 Ibrahim Abdelkader <iabdalkader@openmv.io>
 * Copyright (c) 2013-2021 Kwabena W. Agyeman <kwagyeman@openmv.io>
 *
 * This work is licensed under the MIT license, see the file LICENSE for details.
 *
 * OV5640 driver.
 */
#include "cam_boardconfig.h"
#if (OMV_ENABLE_OV5640 == 1)

#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include "omv_i2c.h"
#include "sensor.h"
#include "ov5640.h"
#include "ov5640_regs.h"

#define BLANK_LINES             8
#define DUMMY_LINES             6

#define BLANK_COLUMNS           0
#define DUMMY_COLUMNS           16

#define SENSOR_WIDTH            2624
#define SENSOR_HEIGHT           1964

#define ACTIVE_SENSOR_WIDTH     (SENSOR_WIDTH - BLANK_COLUMNS - (2 * DUMMY_COLUMNS))
#define ACTIVE_SENSOR_HEIGHT    (SENSOR_HEIGHT - BLANK_LINES - (2 * DUMMY_LINES))

#define DUMMY_WIDTH_BUFFER      16
#define DUMMY_HEIGHT_BUFFER     8

#define HSYNC_TIME              252
#define VYSNC_TIME              24

static int16_t readout_x = 0;
static int16_t readout_y = 0;

static uint16_t readout_w = ACTIVE_SENSOR_WIDTH;
static uint16_t readout_h = ACTIVE_SENSOR_HEIGHT;

static uint16_t hts_target = 0;

static const uint8_t default_regs[][3] = {

// https://github.com/ArduCAM/Arduino/blob/master/ArduCAM/ov5640_regs.h

    { 0x47, 0x40, 0x20 },
    { 0x40, 0x50, 0x6e },
    { 0x40, 0x51, 0x8f },
    { 0x30, 0x08, 0x42 },
    { 0x31, 0x03, 0x03 },
    { 0x30, 0x17, 0xff }, // { 0x30, 0x17, 0x7f },
    { 0x30, 0x18, 0xff },
    { 0x30, 0x2c, 0x02 },
    { 0x31, 0x08, 0x01 },
    { 0x36, 0x30, 0x2e },
    { 0x36, 0x32, 0xe2 },
    { 0x36, 0x33, 0x23 },
    { 0x36, 0x21, 0xe0 },
    { 0x37, 0x04, 0xa0 },
    { 0x37, 0x03, 0x5a },
    { 0x37, 0x15, 0x78 },
    { 0x37, 0x17, 0x01 },
    { 0x37, 0x0b, 0x60 },
    { 0x37, 0x05, 0x1a },
    { 0x39, 0x05, 0x02 },
    { 0x39, 0x06, 0x10 },
    { 0x39, 0x01, 0x0a },
    { 0x37, 0x31, 0x12 },
    { 0x36, 0x00, 0x08 },
    { 0x36, 0x01, 0x33 },
    { 0x30, 0x2d, 0x60 },
    { 0x36, 0x20, 0x52 },
    { 0x37, 0x1b, 0x20 },
    { 0x47, 0x1c, 0x50 },
    { 0x3a, 0x18, 0x00 },
    { 0x3a, 0x19, 0xf8 },
    { 0x36, 0x35, 0x1c },
    { 0x36, 0x34, 0x40 },
    { 0x36, 0x22, 0x01 },
    { 0x3c, 0x04, 0x28 },
    { 0x3c, 0x05, 0x98 },
    { 0x3c, 0x06, 0x00 },
    { 0x3c, 0x07, 0x08 },
    { 0x3c, 0x08, 0x00 },
    { 0x3c, 0x09, 0x1c },
    { 0x3c, 0x0a, 0x9c },
    { 0x3c, 0x0b, 0x40 },
    { 0x38, 0x20, 0x47 }, // { 0x38, 0x20, 0x41 },
    { 0x38, 0x21, 0x01 },
    { 0x38, 0x00, 0x00 },
    { 0x38, 0x01, 0x00 },
    { 0x38, 0x02, 0x00 },
    { 0x38, 0x03, 0x04 },
    { 0x38, 0x04, 0x0a },
    { 0x38, 0x05, 0x3f },
    { 0x38, 0x06, 0x07 },
    { 0x38, 0x07, 0x9b },
    { 0x38, 0x08, 0x05 },
    { 0x38, 0x09, 0x00 },
    { 0x38, 0x0a, 0x03 },
    { 0x38, 0x0b, 0xc0 },
    { 0x38, 0x10, 0x00 },
    { 0x38, 0x11, 0x10 },
    { 0x38, 0x12, 0x00 },
    { 0x38, 0x13, 0x06 },
    { 0x38, 0x14, 0x31 },
    { 0x38, 0x15, 0x31 },
    { 0x30, 0x34, 0x1a },
    { 0x30, 0x35, 0x11 }, // { 0x30, 0x35, 0x21 },
    { 0x30, 0x36, OMV_OV5640_PLL_CTRL2 }, // { 0x30, 0x36, 0x46 },
    { 0x30, 0x37, OMV_OV5640_PLL_CTRL3 },
    { 0x30, 0x38, 0x00 },
    { 0x30, 0x39, 0x00 },
    { 0x38, 0x0c, 0x07 },
    { 0x38, 0x0d, 0x68 },
    { 0x38, 0x0e, 0x03 },
    { 0x38, 0x0f, 0xd8 },
    { 0x3c, 0x01, 0xb4 },
    { 0x3c, 0x00, 0x04 },
    { 0x3a, 0x08, 0x00 },
    { 0x3a, 0x09, 0x93 },
    { 0x3a, 0x0e, 0x06 },
    { 0x3a, 0x0a, 0x00 },
    { 0x3a, 0x0b, 0x7b },
    { 0x3a, 0x0d, 0x08 },
    { 0x3a, 0x00, 0x38 }, // { 0x3a, 0x00, 0x3c },
    { 0x3a, 0x02, 0x05 },
    { 0x3a, 0x03, 0xc4 },
    { 0x3a, 0x14, 0x05 },
    { 0x3a, 0x15, 0xc4 },
    { 0x36, 0x18, 0x00 },
    { 0x36, 0x12, 0x29 },
    { 0x37, 0x08, 0x64 },
    { 0x37, 0x09, 0x52 },
    { 0x37, 0x0c, 0x03 },
    { 0x40, 0x01, 0x02 },
    { 0x40, 0x04, 0x02 },
    { 0x30, 0x00, 0x00 },
    { 0x30, 0x02, 0x1c },
    { 0x30, 0x04, 0xff },
    { 0x30, 0x06, 0xc3 },
    { 0x30, 0x0e, 0x58 },
    { 0x30, 0x2e, 0x00 },
    { 0x43, 0x00, 0x30 },
    { 0x50, 0x1f, 0x00 },
    { 0x47, 0x13, 0x04 }, // { 0x47, 0x13, 0x03 },
    { 0x44, 0x07, 0x04 },
    { 0x46, 0x0b, 0x35 },
    { 0x46, 0x0c, 0x22 },
    { 0x38, 0x24, 0x02 }, // { 0x38, 0x24, 0x01 },
    { 0x50, 0x01, 0xa3 },
    { 0x34, 0x06, 0x01 },
    { 0x34, 0x00, 0x06 },
    { 0x34, 0x01, 0x80 },
    { 0x34, 0x02, 0x04 },
    { 0x34, 0x03, 0x00 },
    { 0x34, 0x04, 0x06 },
    { 0x34, 0x05, 0x00 },
    { 0x51, 0x80, 0xff },
    { 0x51, 0x81, 0xf2 },
    { 0x51, 0x82, 0x00 },
    { 0x51, 0x83, 0x14 },
    { 0x51, 0x84, 0x25 },
    { 0x51, 0x85, 0x24 },
    { 0x51, 0x86, 0x16 },
    { 0x51, 0x87, 0x16 },
    { 0x51, 0x88, 0x16 },
    { 0x51, 0x89, 0x62 },
    { 0x51, 0x8a, 0x62 },
    { 0x51, 0x8b, 0xf0 },
    { 0x51, 0x8c, 0xb2 },
    { 0x51, 0x8d, 0x50 },
    { 0x51, 0x8e, 0x30 },
    { 0x51, 0x8f, 0x30 },
    { 0x51, 0x90, 0x50 },
    { 0x51, 0x91, 0xf8 },
    { 0x51, 0x92, 0x04 },
    { 0x51, 0x93, 0x70 },
    { 0x51, 0x94, 0xf0 },
    { 0x51, 0x95, 0xf0 },
    { 0x51, 0x96, 0x03 },
    { 0x51, 0x97, 0x01 },
    { 0x51, 0x98, 0x04 },
    { 0x51, 0x99, 0x12 },
    { 0x51, 0x9a, 0x04 },
    { 0x51, 0x9b, 0x00 },
    { 0x51, 0x9c, 0x06 },
    { 0x51, 0x9d, 0x82 },
    { 0x51, 0x9e, 0x38 },
    { 0x53, 0x81, 0x1e },
    { 0x53, 0x82, 0x5b },
    { 0x53, 0x83, 0x14 },
    { 0x53, 0x84, 0x06 },
    { 0x53, 0x85, 0x82 },
    { 0x53, 0x86, 0x88 },
    { 0x53, 0x87, 0x7c },
    { 0x53, 0x88, 0x60 },
    { 0x53, 0x89, 0x1c },
    { 0x53, 0x8a, 0x01 },
    { 0x53, 0x8b, 0x98 },
    { 0x53, 0x00, 0x08 },
    { 0x53, 0x01, 0x30 },
    { 0x53, 0x02, 0x3f },
    { 0x53, 0x03, 0x10 },
    { 0x53, 0x04, 0x08 },
    { 0x53, 0x05, 0x30 },
    { 0x53, 0x06, 0x18 },
    { 0x53, 0x07, 0x28 },
    { 0x53, 0x09, 0x08 },
    { 0x53, 0x0a, 0x30 },
    { 0x53, 0x0b, 0x04 },
    { 0x53, 0x0c, 0x06 },
    { 0x54, 0x80, 0x01 },
    { 0x54, 0x81, 0x06 },
    { 0x54, 0x82, 0x12 },
    { 0x54, 0x83, 0x24 },
    { 0x54, 0x84, 0x4a },
    { 0x54, 0x85, 0x58 },
    { 0x54, 0x86, 0x65 },
    { 0x54, 0x87, 0x72 },
    { 0x54, 0x88, 0x7d },
    { 0x54, 0x89, 0x88 },
    { 0x54, 0x8a, 0x92 },
    { 0x54, 0x8b, 0xa3 },
    { 0x54, 0x8c, 0xb2 },
    { 0x54, 0x8d, 0xc8 },
    { 0x54, 0x8e, 0xdd },
    { 0x54, 0x8f, 0xf0 },
    { 0x54, 0x90, 0x15 },
    { 0x55, 0x80, 0x06 },
    { 0x55, 0x83, 0x40 },
    { 0x55, 0x84, 0x20 },
    { 0x55, 0x89, 0x10 },
    { 0x55, 0x8a, 0x00 },
    { 0x55, 0x8b, 0xf8 },
    { 0x50, 0x00, 0x27 }, // { 0x50, 0x00, 0xa7 },
    { 0x58, 0x00, 0x20 },
    { 0x58, 0x01, 0x19 },
    { 0x58, 0x02, 0x17 },
    { 0x58, 0x03, 0x16 },
    { 0x58, 0x04, 0x18 },
    { 0x58, 0x05, 0x21 },
    { 0x58, 0x06, 0x0F },
    { 0x58, 0x07, 0x0A },
    { 0x58, 0x08, 0x07 },
    { 0x58, 0x09, 0x07 },
    { 0x58, 0x0a, 0x0A },
    { 0x58, 0x0b, 0x0C },
    { 0x58, 0x0c, 0x0A },
    { 0x58, 0x0d, 0x03 },
    { 0x58, 0x0e, 0x01 },
    { 0x58, 0x0f, 0x01 },
    { 0x58, 0x10, 0x03 },
    { 0x58, 0x11, 0x09 },
    { 0x58, 0x12, 0x0A },
    { 0x58, 0x13, 0x03 },
    { 0x58, 0x14, 0x01 },
    { 0x58, 0x15, 0x01 },
    { 0x58, 0x16, 0x03 },
    { 0x58, 0x17, 0x08 },
    { 0x58, 0x18, 0x10 },
    { 0x58, 0x19, 0x0A },
    { 0x58, 0x1a, 0x06 },
    { 0x58, 0x1b, 0x06 },
    { 0x58, 0x1c, 0x08 },
    { 0x58, 0x1d, 0x0E },
    { 0x58, 0x1e, 0x22 },
    { 0x58, 0x1f, 0x18 },
    { 0x58, 0x20, 0x13 },
    { 0x58, 0x21, 0x12 },
    { 0x58, 0x22, 0x16 },
    { 0x58, 0x23, 0x1E },
    { 0x58, 0x24, 0x64 },
    { 0x58, 0x25, 0x2A },
    { 0x58, 0x26, 0x2C },
    { 0x58, 0x27, 0x2A },
    { 0x58, 0x28, 0x46 },
    { 0x58, 0x29, 0x2A },
    { 0x58, 0x2a, 0x26 },
    { 0x58, 0x2b, 0x24 },
    { 0x58, 0x2c, 0x26 },
    { 0x58, 0x2d, 0x2A },
    { 0x58, 0x2e, 0x28 },
    { 0x58, 0x2f, 0x42 },
    { 0x58, 0x30, 0x40 },
    { 0x58, 0x31, 0x42 },
    { 0x58, 0x32, 0x08 },
    { 0x58, 0x33, 0x28 },
    { 0x58, 0x34, 0x26 },
    { 0x58, 0x35, 0x24 },
    { 0x58, 0x36, 0x26 },
    { 0x58, 0x37, 0x2A },
    { 0x58, 0x38, 0x44 },
    { 0x58, 0x39, 0x4A },
    { 0x58, 0x3a, 0x2C },
    { 0x58, 0x3b, 0x2a },
    { 0x58, 0x3c, 0x46 },
    { 0x58, 0x3d, 0xCE },
    { 0x56, 0x88, 0x11 }, // { 0x56, 0x88, 0x22 },
    { 0x56, 0x89, 0x11 }, // { 0x56, 0x89, 0x22 },
    { 0x56, 0x8a, 0x11 }, // { 0x56, 0x8a, 0x42 },
    { 0x56, 0x8b, 0x11 }, // { 0x56, 0x8b, 0x24 },
    { 0x56, 0x8c, 0x11 }, // { 0x56, 0x8c, 0x42 },
    { 0x56, 0x8d, 0x11 }, // { 0x56, 0x8d, 0x24 },
    { 0x56, 0x8e, 0x11 }, // { 0x56, 0x8e, 0x22 },
    { 0x56, 0x8f, 0x11 }, // { 0x56, 0x8f, 0x22 },
    { 0x50, 0x25, 0x00 },
    { 0x3a, 0x0f, 0x42 }, // { 0x3a, 0x0f, 0x30 },
    { 0x3a, 0x10, 0x38 }, // { 0x3a, 0x10, 0x28 },
    { 0x3a, 0x1b, 0x44 }, // { 0x3a, 0x1b, 0x30 },
    { 0x3a, 0x1e, 0x36 }, // { 0x3a, 0x1e, 0x28 },
    { 0x3a, 0x11, 0x60 }, // { 0x3a, 0x11, 0x61 },
    { 0x3a, 0x1f, 0x10 },
    { 0x40, 0x05, 0x1a },
    { 0x34, 0x06, 0x00 },
    { 0x35, 0x03, 0x00 },
    { 0x30, 0x08, 0x02 },

// OpenMV Custom.

    { 0x3a, 0x02, 0x07 },
    { 0x3a, 0x03, 0xae },
    { 0x3a, 0x08, 0x01 },
    { 0x3a, 0x09, 0x27 },
    { 0x3a, 0x0a, 0x00 },
    { 0x3a, 0x0b, 0xf6 },
    { 0x3a, 0x0e, 0x06 },
    { 0x3a, 0x0d, 0x08 },
    { 0x3a, 0x14, 0x07 },
    { 0x3a, 0x15, 0xae },
    { 0x44, 0x01, 0x0d }, // | Read SRAM enable when blanking | Read SRAM at first blanking
    { 0x47, 0x23, 0x03 }, // DVP JPEG Mode456 Skip Line Number

// End.

    { 0x00, 0x00, 0x00 }
};

#if (OMV_ENABLE_OV5640_AF == 1)
static const uint8_t af_firmware_regs[] = {
    0x02, 0x0f, 0xd6, 0x02, 0x0a, 0x39, 0xc2, 0x01, 0x22, 0x22, 0x00, 0x02, 0x0f, 0xb2, 0xe5, 0x1f,
    0x70, 0x72, 0xf5, 0x1e, 0xd2, 0x35, 0xff, 0xef, 0x25, 0xe0, 0x24, 0x4e, 0xf8, 0xe4, 0xf6, 0x08,
    0xf6, 0x0f, 0xbf, 0x34, 0xf2, 0x90, 0x0e, 0x93, 0xe4, 0x93, 0xff, 0xe5, 0x4b, 0xc3, 0x9f, 0x50,
    0x04, 0x7f, 0x05, 0x80, 0x02, 0x7f, 0xfb, 0x78, 0xbd, 0xa6, 0x07, 0x12, 0x0f, 0x04, 0x40, 0x04,
    0x7f, 0x03, 0x80, 0x02, 0x7f, 0x30, 0x78, 0xbc, 0xa6, 0x07, 0xe6, 0x18, 0xf6, 0x08, 0xe6, 0x78,
    0xb9, 0xf6, 0x78, 0xbc, 0xe6, 0x78, 0xba, 0xf6, 0x78, 0xbf, 0x76, 0x33, 0xe4, 0x08, 0xf6, 0x78,
    0xb8, 0x76, 0x01, 0x75, 0x4a, 0x02, 0x78, 0xb6, 0xf6, 0x08, 0xf6, 0x74, 0xff, 0x78, 0xc1, 0xf6,
    0x08, 0xf6, 0x75, 0x1f, 0x01, 0x78, 0xbc, 0xe6, 0x75, 0xf0, 0x05, 0xa4, 0xf5, 0x4b, 0x12, 0x0a,
    0xff, 0xc2, 0x37, 0x22, 0x78, 0xb8, 0xe6, 0xd3, 0x94, 0x00, 0x40, 0x02, 0x16, 0x22, 0xe5, 0x1f,
    0xb4, 0x05, 0x23, 0xe4, 0xf5, 0x1f, 0xc2, 0x01, 0x78, 0xb6, 0xe6, 0xfe, 0x08, 0xe6, 0xff, 0x78,
    0x4e, 0xa6, 0x06, 0x08, 0xa6, 0x07, 0xa2, 0x37, 0xe4, 0x33, 0xf5, 0x3c, 0x90, 0x30, 0x28, 0xf0,
    0x75, 0x1e, 0x10, 0xd2, 0x35, 0x22, 0xe5, 0x4b, 0x75, 0xf0, 0x05, 0x84, 0x78, 0xbc, 0xf6, 0x90,
    0x0e, 0x8c, 0xe4, 0x93, 0xff, 0x25, 0xe0, 0x24, 0x0a, 0xf8, 0xe6, 0xfc, 0x08, 0xe6, 0xfd, 0x78,
    0xbc, 0xe6, 0x25, 0xe0, 0x24, 0x4e, 0xf8, 0xa6, 0x04, 0x08, 0xa6, 0x05, 0xef, 0x12, 0x0f, 0x0b,
    0xd3, 0x78, 0xb7, 0x96, 0xee, 0x18, 0x96, 0x40, 0x0d, 0x78, 0xbc, 0xe6, 0x78, 0xb9, 0xf6, 0x78,
    0xb6, 0xa6, 0x06, 0x08, 0xa6, 0x07, 0x90, 0x0e, 0x8c, 0xe4, 0x93, 0x12, 0x0f, 0x0b, 0xc3, 0x78,
    0xc2, 0x96, 0xee, 0x18, 0x96, 0x50, 0x0d, 0x78, 0xbc, 0xe6, 0x78, 0xba, 0xf6, 0x78, 0xc1, 0xa6,
    0x06, 0x08, 0xa6, 0x07, 0x78, 0xb6, 0xe6, 0xfe, 0x08, 0xe6, 0xc3, 0x78, 0xc2, 0x96, 0xff, 0xee,
    0x18, 0x96, 0x78, 0xc3, 0xf6, 0x08, 0xa6, 0x07, 0x90, 0x0e, 0x95, 0xe4, 0x18, 0x12, 0x0e, 0xe9,
    0x40, 0x02, 0xd2, 0x37, 0x78, 0xbc, 0xe6, 0x08, 0x26, 0x08, 0xf6, 0xe5, 0x1f, 0x64, 0x01, 0x70,
    0x4a, 0xe6, 0xc3, 0x78, 0xc0, 0x12, 0x0e, 0xdf, 0x40, 0x05, 0x12, 0x0e, 0xda, 0x40, 0x39, 0x12,
    0x0f, 0x02, 0x40, 0x04, 0x7f, 0xfe, 0x80, 0x02, 0x7f, 0x02, 0x78, 0xbd, 0xa6, 0x07, 0x78, 0xb9,
    0xe6, 0x24, 0x03, 0x78, 0xbf, 0xf6, 0x78, 0xb9, 0xe6, 0x24, 0xfd, 0x78, 0xc0, 0xf6, 0x12, 0x0f,
    0x02, 0x40, 0x06, 0x78, 0xc0, 0xe6, 0xff, 0x80, 0x04, 0x78, 0xbf, 0xe6, 0xff, 0x78, 0xbe, 0xa6,
    0x07, 0x75, 0x1f, 0x02, 0x78, 0xb8, 0x76, 0x01, 0x02, 0x02, 0x4a, 0xe5, 0x1f, 0x64, 0x02, 0x60,
    0x03, 0x02, 0x02, 0x2a, 0x78, 0xbe, 0xe6, 0xff, 0xc3, 0x78, 0xc0, 0x12, 0x0e, 0xe0, 0x40, 0x08,
    0x12, 0x0e, 0xda, 0x50, 0x03, 0x02, 0x02, 0x28, 0x12, 0x0f, 0x02, 0x40, 0x04, 0x7f, 0xff, 0x80,
    0x02, 0x7f, 0x01, 0x78, 0xbd, 0xa6, 0x07, 0x78, 0xb9, 0xe6, 0x04, 0x78, 0xbf, 0xf6, 0x78, 0xb9,
    0xe6, 0x14, 0x78, 0xc0, 0xf6, 0x18, 0x12, 0x0f, 0x04, 0x40, 0x04, 0xe6, 0xff, 0x80, 0x02, 0x7f,
    0x00, 0x78, 0xbf, 0xa6, 0x07, 0xd3, 0x08, 0xe6, 0x64, 0x80, 0x94, 0x80, 0x40, 0x04, 0xe6, 0xff,
    0x80, 0x02, 0x7f, 0x00, 0x78, 0xc0, 0xa6, 0x07, 0xc3, 0x18, 0xe6, 0x64, 0x80, 0x94, 0xb3, 0x50,
    0x04, 0xe6, 0xff, 0x80, 0x02, 0x7f, 0x33, 0x78, 0xbf, 0xa6, 0x07, 0xc3, 0x08, 0xe6, 0x64, 0x80,
    0x94, 0xb3, 0x50, 0x04, 0xe6, 0xff, 0x80, 0x02, 0x7f, 0x33, 0x78, 0xc0, 0xa6, 0x07, 0x12, 0x0f,
    0x02, 0x40, 0x06, 0x78, 0xc0, 0xe6, 0xff, 0x80, 0x04, 0x78, 0xbf, 0xe6, 0xff, 0x78, 0xbe, 0xa6,
    0x07, 0x75, 0x1f, 0x03, 0x78, 0xb8, 0x76, 0x01, 0x80, 0x20, 0xe5, 0x1f, 0x64, 0x03, 0x70, 0x26,
    0x78, 0xbe, 0xe6, 0xff, 0xc3, 0x78, 0xc0, 0x12, 0x0e, 0xe0, 0x40, 0x05, 0x12, 0x0e, 0xda, 0x40,
    0x09, 0x78, 0xb9, 0xe6, 0x78, 0xbe, 0xf6, 0x75, 0x1f, 0x04, 0x78, 0xbe, 0xe6, 0x75, 0xf0, 0x05,
    0xa4, 0xf5, 0x4b, 0x02, 0x0a, 0xff, 0xe5, 0x1f, 0xb4, 0x04, 0x10, 0x90, 0x0e, 0x94, 0xe4, 0x78,
    0xc3, 0x12, 0x0e, 0xe9, 0x40, 0x02, 0xd2, 0x37, 0x75, 0x1f, 0x05, 0x22, 0x30, 0x01, 0x03, 0x02,
    0x04, 0xc0, 0x30, 0x02, 0x03, 0x02, 0x04, 0xc0, 0x90, 0x51, 0xa5, 0xe0, 0x78, 0x93, 0xf6, 0xa3,
    0xe0, 0x08, 0xf6, 0xa3, 0xe0, 0x08, 0xf6, 0xe5, 0x1f, 0x70, 0x3c, 0x75, 0x1e, 0x20, 0xd2, 0x35,
    0x12, 0x0c, 0x7a, 0x78, 0x7e, 0xa6, 0x06, 0x08, 0xa6, 0x07, 0x78, 0x8b, 0xa6, 0x09, 0x18, 0x76,
    0x01, 0x12, 0x0c, 0x5b, 0x78, 0x4e, 0xa6, 0x06, 0x08, 0xa6, 0x07, 0x78, 0x8b, 0xe6, 0x78, 0x6e,
    0xf6, 0x75, 0x1f, 0x01, 0x78, 0x93, 0xe6, 0x78, 0x90, 0xf6, 0x78, 0x94, 0xe6, 0x78, 0x91, 0xf6,
    0x78, 0x95, 0xe6, 0x78, 0x92, 0xf6, 0x22, 0x79, 0x90, 0xe7, 0xd3, 0x78, 0x93, 0x96, 0x40, 0x05,
    0xe7, 0x96, 0xff, 0x80, 0x08, 0xc3, 0x79, 0x93, 0xe7, 0x78, 0x90, 0x96, 0xff, 0x78, 0x88, 0x76,
    0x00, 0x08, 0xa6, 0x07, 0x79, 0x91, 0xe7, 0xd3, 0x78, 0x94, 0x96, 0x40, 0x05, 0xe7, 0x96, 0xff,
    0x80, 0x08, 0xc3, 0x79, 0x94, 0xe7, 0x78, 0x91, 0x96, 0xff, 0x12, 0x0c, 0x8e, 0x79, 0x92, 0xe7,
    0xd3, 0x78, 0x95, 0x96, 0x40, 0x05, 0xe7, 0x96, 0xff, 0x80, 0x08, 0xc3, 0x79, 0x95, 0xe7, 0x78,
    0x92, 0x96, 0xff, 0x12, 0x0c, 0x8e, 0x12, 0x0c, 0x5b, 0x78, 0x8a, 0xe6, 0x25, 0xe0, 0x24, 0x4e,
    0xf8, 0xa6, 0x06, 0x08, 0xa6, 0x07, 0x78, 0x8a, 0xe6, 0x24, 0x6e, 0xf8, 0xa6, 0x09, 0x78, 0x8a,
    0xe6, 0x24, 0x01, 0xff, 0xe4, 0x33, 0xfe, 0xd3, 0xef, 0x94, 0x0f, 0xee, 0x64, 0x80, 0x94, 0x80,
    0x40, 0x04, 0x7f, 0x00, 0x80, 0x05, 0x78, 0x8a, 0xe6, 0x04, 0xff, 0x78, 0x8a, 0xa6, 0x07, 0xe5,
    0x1f, 0xb4, 0x01, 0x0a, 0xe6, 0x60, 0x03, 0x02, 0x04, 0xc0, 0x75, 0x1f, 0x02, 0x22, 0x12, 0x0c,
    0x7a, 0x78, 0x80, 0xa6, 0x06, 0x08, 0xa6, 0x07, 0x12, 0x0c, 0x7a, 0x78, 0x82, 0xa6, 0x06, 0x08,
    0xa6, 0x07, 0x78, 0x6e, 0xe6, 0x78, 0x8c, 0xf6, 0x78, 0x6e, 0xe6, 0x78, 0x8d, 0xf6, 0x7f, 0x01,
    0xef, 0x25, 0xe0, 0x24, 0x4f, 0xf9, 0xc3, 0x78, 0x81, 0xe6, 0x97, 0x18, 0xe6, 0x19, 0x97, 0x50,
    0x0a, 0x12, 0x0c, 0x82, 0x78, 0x80, 0xa6, 0x04, 0x08, 0xa6, 0x05, 0x74, 0x6e, 0x2f, 0xf9, 0x78,
    0x8c, 0xe6, 0xc3, 0x97, 0x50, 0x08, 0x74, 0x6e, 0x2f, 0xf8, 0xe6, 0x78, 0x8c, 0xf6, 0xef, 0x25,
    0xe0, 0x24, 0x4f, 0xf9, 0xd3, 0x78, 0x83, 0xe6, 0x97, 0x18, 0xe6, 0x19, 0x97, 0x40, 0x0a, 0x12,
    0x0c, 0x82, 0x78, 0x82, 0xa6, 0x04, 0x08, 0xa6, 0x05, 0x74, 0x6e, 0x2f, 0xf9, 0x78, 0x8d, 0xe6,
    0xd3, 0x97, 0x40, 0x08, 0x74, 0x6e, 0x2f, 0xf8, 0xe6, 0x78, 0x8d, 0xf6, 0x0f, 0xef, 0x64, 0x10,
    0x70, 0x9e, 0xc3, 0x79, 0x81, 0xe7, 0x78, 0x83, 0x96, 0xff, 0x19, 0xe7, 0x18, 0x96, 0x78, 0x84,
    0xf6, 0x08, 0xa6, 0x07, 0xc3, 0x79, 0x8c, 0xe7, 0x78, 0x8d, 0x96, 0x08, 0xf6, 0xd3, 0x79, 0x81,
    0xe7, 0x78, 0x7f, 0x96, 0x19, 0xe7, 0x18, 0x96, 0x40, 0x05, 0x09, 0xe7, 0x08, 0x80, 0x06, 0xc3,
    0x79, 0x7f, 0xe7, 0x78, 0x81, 0x96, 0xff, 0x19, 0xe7, 0x18, 0x96, 0xfe, 0x78, 0x86, 0xa6, 0x06,
    0x08, 0xa6, 0x07, 0x79, 0x8c, 0xe7, 0xd3, 0x78, 0x8b, 0x96, 0x40, 0x05, 0xe7, 0x96, 0xff, 0x80,
    0x08, 0xc3, 0x79, 0x8b, 0xe7, 0x78, 0x8c, 0x96, 0xff, 0x78, 0x8f, 0xa6, 0x07, 0xe5, 0x1f, 0x64,
    0x02, 0x70, 0x69, 0x90, 0x0e, 0x91, 0x93, 0xff, 0x18, 0xe6, 0xc3, 0x9f, 0x50, 0x72, 0x12, 0x0c,
    0x4a, 0x12, 0x0c, 0x2f, 0x90, 0x0e, 0x8e, 0x12, 0x0c, 0x38, 0x78, 0x80, 0x12, 0x0c, 0x6b, 0x7b,
    0x04, 0x12, 0x0c, 0x1d, 0xc3, 0x12, 0x06, 0x45, 0x50, 0x56, 0x90, 0x0e, 0x92, 0xe4, 0x93, 0xff,
    0x78, 0x8f, 0xe6, 0x9f, 0x40, 0x02, 0x80, 0x11, 0x90, 0x0e, 0x90, 0xe4, 0x93, 0xff, 0xd3, 0x78,
    0x89, 0xe6, 0x9f, 0x18, 0xe6, 0x94, 0x00, 0x40, 0x03, 0x75, 0x1f, 0x05, 0x12, 0x0c, 0x4a, 0x12,
    0x0c, 0x2f, 0x90, 0x0e, 0x8f, 0x12, 0x0c, 0x38, 0x78, 0x7e, 0x12, 0x0c, 0x6b, 0x7b, 0x40, 0x12,
    0x0c, 0x1d, 0xd3, 0x12, 0x06, 0x45, 0x40, 0x18, 0x75, 0x1f, 0x05, 0x22, 0xe5, 0x1f, 0xb4, 0x05,
    0x0f, 0xd2, 0x01, 0xc2, 0x02, 0xe4, 0xf5, 0x1f, 0xf5, 0x1e, 0xd2, 0x35, 0xd2, 0x33, 0xd2, 0x36,
    0x22, 0xef, 0x8d, 0xf0, 0xa4, 0xa8, 0xf0, 0xcf, 0x8c, 0xf0, 0xa4, 0x28, 0xce, 0x8d, 0xf0, 0xa4,
    0x2e, 0xfe, 0x22, 0xbc, 0x00, 0x0b, 0xbe, 0x00, 0x29, 0xef, 0x8d, 0xf0, 0x84, 0xff, 0xad, 0xf0,
    0x22, 0xe4, 0xcc, 0xf8, 0x75, 0xf0, 0x08, 0xef, 0x2f, 0xff, 0xee, 0x33, 0xfe, 0xec, 0x33, 0xfc,
    0xee, 0x9d, 0xec, 0x98, 0x40, 0x05, 0xfc, 0xee, 0x9d, 0xfe, 0x0f, 0xd5, 0xf0, 0xe9, 0xe4, 0xce,
    0xfd, 0x22, 0xed, 0xf8, 0xf5, 0xf0, 0xee, 0x84, 0x20, 0xd2, 0x1c, 0xfe, 0xad, 0xf0, 0x75, 0xf0,
    0x08, 0xef, 0x2f, 0xff, 0xed, 0x33, 0xfd, 0x40, 0x07, 0x98, 0x50, 0x06, 0xd5, 0xf0, 0xf2, 0x22,
    0xc3, 0x98, 0xfd, 0x0f, 0xd5, 0xf0, 0xea, 0x22, 0xe8, 0x8f, 0xf0, 0xa4, 0xcc, 0x8b, 0xf0, 0xa4,
    0x2c, 0xfc, 0xe9, 0x8e, 0xf0, 0xa4, 0x2c, 0xfc, 0x8a, 0xf0, 0xed, 0xa4, 0x2c, 0xfc, 0xea, 0x8e,
    0xf0, 0xa4, 0xcd, 0xa8, 0xf0, 0x8b, 0xf0, 0xa4, 0x2d, 0xcc, 0x38, 0x25, 0xf0, 0xfd, 0xe9, 0x8f,
    0xf0, 0xa4, 0x2c, 0xcd, 0x35, 0xf0, 0xfc, 0xeb, 0x8e, 0xf0, 0xa4, 0xfe, 0xa9, 0xf0, 0xeb, 0x8f,
    0xf0, 0xa4, 0xcf, 0xc5, 0xf0, 0x2e, 0xcd, 0x39, 0xfe, 0xe4, 0x3c, 0xfc, 0xea, 0xa4, 0x2d, 0xce,
    0x35, 0xf0, 0xfd, 0xe4, 0x3c, 0xfc, 0x22, 0x75, 0xf0, 0x08, 0x75, 0x82, 0x00, 0xef, 0x2f, 0xff,
    0xee, 0x33, 0xfe, 0xcd, 0x33, 0xcd, 0xcc, 0x33, 0xcc, 0xc5, 0x82, 0x33, 0xc5, 0x82, 0x9b, 0xed,
    0x9a, 0xec, 0x99, 0xe5, 0x82, 0x98, 0x40, 0x0c, 0xf5, 0x82, 0xee, 0x9b, 0xfe, 0xed, 0x9a, 0xfd,
    0xec, 0x99, 0xfc, 0x0f, 0xd5, 0xf0, 0xd6, 0xe4, 0xce, 0xfb, 0xe4, 0xcd, 0xfa, 0xe4, 0xcc, 0xf9,
    0xa8, 0x82, 0x22, 0xb8, 0x00, 0xc1, 0xb9, 0x00, 0x59, 0xba, 0x00, 0x2d, 0xec, 0x8b, 0xf0, 0x84,
    0xcf, 0xce, 0xcd, 0xfc, 0xe5, 0xf0, 0xcb, 0xf9, 0x78, 0x18, 0xef, 0x2f, 0xff, 0xee, 0x33, 0xfe,
    0xed, 0x33, 0xfd, 0xec, 0x33, 0xfc, 0xeb, 0x33, 0xfb, 0x10, 0xd7, 0x03, 0x99, 0x40, 0x04, 0xeb,
    0x99, 0xfb, 0x0f, 0xd8, 0xe5, 0xe4, 0xf9, 0xfa, 0x22, 0x78, 0x18, 0xef, 0x2f, 0xff, 0xee, 0x33,
    0xfe, 0xed, 0x33, 0xfd, 0xec, 0x33, 0xfc, 0xc9, 0x33, 0xc9, 0x10, 0xd7, 0x05, 0x9b, 0xe9, 0x9a,
    0x40, 0x07, 0xec, 0x9b, 0xfc, 0xe9, 0x9a, 0xf9, 0x0f, 0xd8, 0xe0, 0xe4, 0xc9, 0xfa, 0xe4, 0xcc,
    0xfb, 0x22, 0x75, 0xf0, 0x10, 0xef, 0x2f, 0xff, 0xee, 0x33, 0xfe, 0xed, 0x33, 0xfd, 0xcc, 0x33,
    0xcc, 0xc8, 0x33, 0xc8, 0x10, 0xd7, 0x07, 0x9b, 0xec, 0x9a, 0xe8, 0x99, 0x40, 0x0a, 0xed, 0x9b,
    0xfd, 0xec, 0x9a, 0xfc, 0xe8, 0x99, 0xf8, 0x0f, 0xd5, 0xf0, 0xda, 0xe4, 0xcd, 0xfb, 0xe4, 0xcc,
    0xfa, 0xe4, 0xc8, 0xf9, 0x22, 0xeb, 0x9f, 0xf5, 0xf0, 0xea, 0x9e, 0x42, 0xf0, 0xe9, 0x9d, 0x42,
    0xf0, 0xe8, 0x9c, 0x45, 0xf0, 0x22, 0xe8, 0x60, 0x0f, 0xec, 0xc3, 0x13, 0xfc, 0xed, 0x13, 0xfd,
    0xee, 0x13, 0xfe, 0xef, 0x13, 0xff, 0xd8, 0xf1, 0x22, 0xe8, 0x60, 0x0f, 0xef, 0xc3, 0x33, 0xff,
    0xee, 0x33, 0xfe, 0xed, 0x33, 0xfd, 0xec, 0x33, 0xfc, 0xd8, 0xf1, 0x22, 0xe4, 0x93, 0xfc, 0x74,
    0x01, 0x93, 0xfd, 0x74, 0x02, 0x93, 0xfe, 0x74, 0x03, 0x93, 0xff, 0x22, 0xe6, 0xfb, 0x08, 0xe6,
    0xf9, 0x08, 0xe6, 0xfa, 0x08, 0xe6, 0xcb, 0xf8, 0x22, 0xec, 0xf6, 0x08, 0xed, 0xf6, 0x08, 0xee,
    0xf6, 0x08, 0xef, 0xf6, 0x22, 0xa4, 0x25, 0x82, 0xf5, 0x82, 0xe5, 0xf0, 0x35, 0x83, 0xf5, 0x83,
    0x22, 0xd0, 0x83, 0xd0, 0x82, 0xf8, 0xe4, 0x93, 0x70, 0x12, 0x74, 0x01, 0x93, 0x70, 0x0d, 0xa3,
    0xa3, 0x93, 0xf8, 0x74, 0x01, 0x93, 0xf5, 0x82, 0x88, 0x83, 0xe4, 0x73, 0x74, 0x02, 0x93, 0x68,
    0x60, 0xef, 0xa3, 0xa3, 0xa3, 0x80, 0xdf, 0x90, 0x38, 0x04, 0x78, 0x52, 0x12, 0x0b, 0xfd, 0x90,
    0x38, 0x00, 0xe0, 0xfe, 0xa3, 0xe0, 0xfd, 0xed, 0xff, 0xc3, 0x12, 0x0b, 0x9e, 0x90, 0x38, 0x10,
    0x12, 0x0b, 0x92, 0x90, 0x38, 0x06, 0x78, 0x54, 0x12, 0x0b, 0xfd, 0x90, 0x38, 0x02, 0xe0, 0xfe,
    0xa3, 0xe0, 0xfd, 0xed, 0xff, 0xc3, 0x12, 0x0b, 0x9e, 0x90, 0x38, 0x12, 0x12, 0x0b, 0x92, 0xa3,
    0xe0, 0xb4, 0x31, 0x07, 0x78, 0x52, 0x79, 0x52, 0x12, 0x0c, 0x13, 0x90, 0x38, 0x14, 0xe0, 0xb4,
    0x71, 0x15, 0x78, 0x52, 0xe6, 0xfe, 0x08, 0xe6, 0x78, 0x02, 0xce, 0xc3, 0x13, 0xce, 0x13, 0xd8,
    0xf9, 0x79, 0x53, 0xf7, 0xee, 0x19, 0xf7, 0x90, 0x38, 0x15, 0xe0, 0xb4, 0x31, 0x07, 0x78, 0x54,
    0x79, 0x54, 0x12, 0x0c, 0x13, 0x90, 0x38, 0x15, 0xe0, 0xb4, 0x71, 0x15, 0x78, 0x54, 0xe6, 0xfe,
    0x08, 0xe6, 0x78, 0x02, 0xce, 0xc3, 0x13, 0xce, 0x13, 0xd8, 0xf9, 0x79, 0x55, 0xf7, 0xee, 0x19,
    0xf7, 0x79, 0x52, 0x12, 0x0b, 0xd9, 0x09, 0x12, 0x0b, 0xd9, 0xaf, 0x47, 0x12, 0x0b, 0xb2, 0xe5,
    0x44, 0xfb, 0x7a, 0x00, 0xfd, 0x7c, 0x00, 0x12, 0x04, 0xd3, 0x78, 0x5a, 0xa6, 0x06, 0x08, 0xa6,
    0x07, 0xaf, 0x45, 0x12, 0x0b, 0xb2, 0xad, 0x03, 0x7c, 0x00, 0x12, 0x04, 0xd3, 0x78, 0x56, 0xa6,
    0x06, 0x08, 0xa6, 0x07, 0xaf, 0x48, 0x78, 0x54, 0x12, 0x0b, 0xb4, 0xe5, 0x43, 0xfb, 0xfd, 0x7c,
    0x00, 0x12, 0x04, 0xd3, 0x78, 0x5c, 0xa6, 0x06, 0x08, 0xa6, 0x07, 0xaf, 0x46, 0x7e, 0x00, 0x78,
    0x54, 0x12, 0x0b, 0xb6, 0xad, 0x03, 0x7c, 0x00, 0x12, 0x04, 0xd3, 0x78, 0x58, 0xa6, 0x06, 0x08,
    0xa6, 0x07, 0xc3, 0x78, 0x5b, 0xe6, 0x94, 0x08, 0x18, 0xe6, 0x94, 0x00, 0x50, 0x05, 0x76, 0x00,
    0x08, 0x76, 0x08, 0xc3, 0x78, 0x5d, 0xe6, 0x94, 0x08, 0x18, 0xe6, 0x94, 0x00, 0x50, 0x05, 0x76,
    0x00, 0x08, 0x76, 0x08, 0x78, 0x5a, 0x12, 0x0b, 0xc6, 0xff, 0xd3, 0x78, 0x57, 0xe6, 0x9f, 0x18,
    0xe6, 0x9e, 0x40, 0x0e, 0x78, 0x5a, 0xe6, 0x13, 0xfe, 0x08, 0xe6, 0x78, 0x57, 0x12, 0x0c, 0x08,
    0x80, 0x04, 0x7e, 0x00, 0x7f, 0x00, 0x78, 0x5e, 0x12, 0x0b, 0xbe, 0xff, 0xd3, 0x78, 0x59, 0xe6,
    0x9f, 0x18, 0xe6, 0x9e, 0x40, 0x0e, 0x78, 0x5c, 0xe6, 0x13, 0xfe, 0x08, 0xe6, 0x78, 0x59, 0x12,
    0x0c, 0x08, 0x80, 0x04, 0x7e, 0x00, 0x7f, 0x00, 0xe4, 0xfc, 0xfd, 0x78, 0x62, 0x12, 0x06, 0x99,
    0x78, 0x5a, 0x12, 0x0b, 0xc6, 0x78, 0x57, 0x26, 0xff, 0xee, 0x18, 0x36, 0xfe, 0x78, 0x66, 0x12,
    0x0b, 0xbe, 0x78, 0x59, 0x26, 0xff, 0xee, 0x18, 0x36, 0xfe, 0xe4, 0xfc, 0xfd, 0x78, 0x6a, 0x12,
    0x06, 0x99, 0x12, 0x0b, 0xce, 0x78, 0x66, 0x12, 0x06, 0x8c, 0xd3, 0x12, 0x06, 0x45, 0x40, 0x08,
    0x12, 0x0b, 0xce, 0x78, 0x66, 0x12, 0x06, 0x99, 0x78, 0x54, 0x12, 0x0b, 0xd0, 0x78, 0x6a, 0x12,
    0x06, 0x8c, 0xd3, 0x12, 0x06, 0x45, 0x40, 0x0a, 0x78, 0x54, 0x12, 0x0b, 0xd0, 0x78, 0x6a, 0x12,
    0x06, 0x99, 0x78, 0x61, 0xe6, 0x90, 0x60, 0x01, 0xf0, 0x78, 0x65, 0xe6, 0xa3, 0xf0, 0x78, 0x69,
    0xe6, 0xa3, 0xf0, 0x78, 0x55, 0xe6, 0xa3, 0xf0, 0x7d, 0x01, 0x78, 0x61, 0x12, 0x0b, 0xe9, 0x24,
    0x01, 0x12, 0x0b, 0xa6, 0x78, 0x65, 0x12, 0x0b, 0xe9, 0x24, 0x02, 0x12, 0x0b, 0xa6, 0x78, 0x69,
    0x12, 0x0b, 0xe9, 0x24, 0x03, 0x12, 0x0b, 0xa6, 0x78, 0x6d, 0x12, 0x0b, 0xe9, 0x24, 0x04, 0x12,
    0x0b, 0xa6, 0x0d, 0xbd, 0x05, 0xd4, 0xc2, 0x0e, 0xc2, 0x06, 0x22, 0x85, 0x08, 0x41, 0x90, 0x30,
    0x24, 0xe0, 0xf5, 0x3d, 0xa3, 0xe0, 0xf5, 0x3e, 0xa3, 0xe0, 0xf5, 0x3f, 0xa3, 0xe0, 0xf5, 0x40,
    0xa3, 0xe0, 0xf5, 0x3c, 0xd2, 0x34, 0xe5, 0x41, 0x12, 0x06, 0xb1, 0x09, 0x31, 0x03, 0x09, 0x35,
    0x04, 0x09, 0x3b, 0x05, 0x09, 0x3e, 0x06, 0x09, 0x41, 0x07, 0x09, 0x4a, 0x08, 0x09, 0x5b, 0x12,
    0x09, 0x73, 0x18, 0x09, 0x89, 0x19, 0x09, 0x5e, 0x1a, 0x09, 0x6a, 0x1b, 0x09, 0xad, 0x80, 0x09,
    0xb2, 0x81, 0x0a, 0x1d, 0x8f, 0x0a, 0x09, 0x90, 0x0a, 0x1d, 0x91, 0x0a, 0x1d, 0x92, 0x0a, 0x1d,
    0x93, 0x0a, 0x1d, 0x94, 0x0a, 0x1d, 0x98, 0x0a, 0x17, 0x9f, 0x0a, 0x1a, 0xec, 0x00, 0x00, 0x0a,
    0x38, 0x12, 0x0f, 0x74, 0x22, 0x12, 0x0f, 0x74, 0xd2, 0x03, 0x22, 0xd2, 0x03, 0x22, 0xc2, 0x03,
    0x22, 0xa2, 0x37, 0xe4, 0x33, 0xf5, 0x3c, 0x02, 0x0a, 0x1d, 0xc2, 0x01, 0xc2, 0x02, 0xc2, 0x03,
    0x12, 0x0d, 0x0d, 0x75, 0x1e, 0x70, 0xd2, 0x35, 0x02, 0x0a, 0x1d, 0x02, 0x0a, 0x04, 0x85, 0x40,
    0x4a, 0x85, 0x3c, 0x4b, 0x12, 0x0a, 0xff, 0x02, 0x0a, 0x1d, 0x85, 0x4a, 0x40, 0x85, 0x4b, 0x3c,
    0x02, 0x0a, 0x1d, 0xe4, 0xf5, 0x22, 0xf5, 0x23, 0x85, 0x40, 0x31, 0x85, 0x3f, 0x30, 0x85, 0x3e,
    0x2f, 0x85, 0x3d, 0x2e, 0x12, 0x0f, 0x46, 0x80, 0x1f, 0x75, 0x22, 0x00, 0x75, 0x23, 0x01, 0x74,
    0xff, 0xf5, 0x2d, 0xf5, 0x2c, 0xf5, 0x2b, 0xf5, 0x2a, 0x12, 0x0f, 0x46, 0x85, 0x2d, 0x40, 0x85,
    0x2c, 0x3f, 0x85, 0x2b, 0x3e, 0x85, 0x2a, 0x3d, 0xe4, 0xf5, 0x3c, 0x80, 0x70, 0x12, 0x0f, 0x16,
    0x80, 0x6b, 0x85, 0x3d, 0x45, 0x85, 0x3e, 0x46, 0xe5, 0x47, 0xc3, 0x13, 0xff, 0xe5, 0x45, 0xc3,
    0x9f, 0x50, 0x02, 0x8f, 0x45, 0xe5, 0x48, 0xc3, 0x13, 0xff, 0xe5, 0x46, 0xc3, 0x9f, 0x50, 0x02,
    0x8f, 0x46, 0xe5, 0x47, 0xc3, 0x13, 0xff, 0xfd, 0xe5, 0x45, 0x2d, 0xfd, 0xe4, 0x33, 0xfc, 0xe5,
    0x44, 0x12, 0x0f, 0x90, 0x40, 0x05, 0xe5, 0x44, 0x9f, 0xf5, 0x45, 0xe5, 0x48, 0xc3, 0x13, 0xff,
    0xfd, 0xe5, 0x46, 0x2d, 0xfd, 0xe4, 0x33, 0xfc, 0xe5, 0x43, 0x12, 0x0f, 0x90, 0x40, 0x05, 0xe5,
    0x43, 0x9f, 0xf5, 0x46, 0x12, 0x06, 0xd7, 0x80, 0x14, 0x85, 0x40, 0x48, 0x85, 0x3f, 0x47, 0x85,
    0x3e, 0x46, 0x85, 0x3d, 0x45, 0x80, 0x06, 0x02, 0x06, 0xd7, 0x12, 0x0d, 0x7e, 0x90, 0x30, 0x24,
    0xe5, 0x3d, 0xf0, 0xa3, 0xe5, 0x3e, 0xf0, 0xa3, 0xe5, 0x3f, 0xf0, 0xa3, 0xe5, 0x40, 0xf0, 0xa3,
    0xe5, 0x3c, 0xf0, 0x90, 0x30, 0x23, 0xe4, 0xf0, 0x22, 0xc0, 0xe0, 0xc0, 0x83, 0xc0, 0x82, 0xc0,
    0xd0, 0x90, 0x3f, 0x0c, 0xe0, 0xf5, 0x32, 0xe5, 0x32, 0x30, 0xe3, 0x74, 0x30, 0x36, 0x66, 0x90,
    0x60, 0x19, 0xe0, 0xf5, 0x0a, 0xa3, 0xe0, 0xf5, 0x0b, 0x90, 0x60, 0x1d, 0xe0, 0xf5, 0x14, 0xa3,
    0xe0, 0xf5, 0x15, 0x90, 0x60, 0x21, 0xe0, 0xf5, 0x0c, 0xa3, 0xe0, 0xf5, 0x0d, 0x90, 0x60, 0x29,
    0xe0, 0xf5, 0x0e, 0xa3, 0xe0, 0xf5, 0x0f, 0x90, 0x60, 0x31, 0xe0, 0xf5, 0x10, 0xa3, 0xe0, 0xf5,
    0x11, 0x90, 0x60, 0x39, 0xe0, 0xf5, 0x12, 0xa3, 0xe0, 0xf5, 0x13, 0x30, 0x01, 0x06, 0x30, 0x33,
    0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0x09, 0x30, 0x02, 0x06, 0x30, 0x33, 0x03, 0xd3, 0x80, 0x01,
    0xc3, 0x92, 0x0a, 0x30, 0x33, 0x0c, 0x30, 0x03, 0x09, 0x20, 0x02, 0x06, 0x20, 0x01, 0x03, 0xd3,
    0x80, 0x01, 0xc3, 0x92, 0x0b, 0x90, 0x30, 0x01, 0xe0, 0x44, 0x40, 0xf0, 0xe0, 0x54, 0xbf, 0xf0,
    0xe5, 0x32, 0x30, 0xe1, 0x14, 0x30, 0x34, 0x11, 0x90, 0x30, 0x22, 0xe0, 0xf5, 0x08, 0xe4, 0xf0,
    0x30, 0x00, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0x08, 0xe5, 0x32, 0x30, 0xe5, 0x12, 0x90, 0x56,
    0xa1, 0xe0, 0xf5, 0x09, 0x30, 0x31, 0x09, 0x30, 0x05, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0x0d,
    0x90, 0x3f, 0x0c, 0xe5, 0x32, 0xf0, 0xd0, 0xd0, 0xd0, 0x82, 0xd0, 0x83, 0xd0, 0xe0, 0x32, 0x90,
    0x0e, 0x7e, 0xe4, 0x93, 0xfe, 0x74, 0x01, 0x93, 0xff, 0xc3, 0x90, 0x0e, 0x7c, 0x74, 0x01, 0x93,
    0x9f, 0xff, 0xe4, 0x93, 0x9e, 0xfe, 0xe4, 0x8f, 0x3b, 0x8e, 0x3a, 0xf5, 0x39, 0xf5, 0x38, 0xab,
    0x3b, 0xaa, 0x3a, 0xa9, 0x39, 0xa8, 0x38, 0xaf, 0x4b, 0xfc, 0xfd, 0xfe, 0x12, 0x05, 0x28, 0x12,
    0x0d, 0xe1, 0xe4, 0x7b, 0xff, 0xfa, 0xf9, 0xf8, 0x12, 0x05, 0xb3, 0x12, 0x0d, 0xe1, 0x90, 0x0e,
    0x69, 0xe4, 0x12, 0x0d, 0xf6, 0x12, 0x0d, 0xe1, 0xe4, 0x85, 0x4a, 0x37, 0xf5, 0x36, 0xf5, 0x35,
    0xf5, 0x34, 0xaf, 0x37, 0xae, 0x36, 0xad, 0x35, 0xac, 0x34, 0xa3, 0x12, 0x0d, 0xf6, 0x8f, 0x37,
    0x8e, 0x36, 0x8d, 0x35, 0x8c, 0x34, 0xe5, 0x3b, 0x45, 0x37, 0xf5, 0x3b, 0xe5, 0x3a, 0x45, 0x36,
    0xf5, 0x3a, 0xe5, 0x39, 0x45, 0x35, 0xf5, 0x39, 0xe5, 0x38, 0x45, 0x34, 0xf5, 0x38, 0xe4, 0xf5,
    0x22, 0xf5, 0x23, 0x85, 0x3b, 0x31, 0x85, 0x3a, 0x30, 0x85, 0x39, 0x2f, 0x85, 0x38, 0x2e, 0x02,
    0x0f, 0x46, 0xe0, 0xa3, 0xe0, 0x75, 0xf0, 0x02, 0xa4, 0xff, 0xae, 0xf0, 0xc3, 0x08, 0xe6, 0x9f,
    0xf6, 0x18, 0xe6, 0x9e, 0xf6, 0x22, 0xff, 0xe5, 0xf0, 0x34, 0x60, 0x8f, 0x82, 0xf5, 0x83, 0xec,
    0xf0, 0x22, 0x78, 0x52, 0x7e, 0x00, 0xe6, 0xfc, 0x08, 0xe6, 0xfd, 0x02, 0x04, 0xc1, 0xe4, 0xfc,
    0xfd, 0x12, 0x06, 0x99, 0x78, 0x5c, 0xe6, 0xc3, 0x13, 0xfe, 0x08, 0xe6, 0x13, 0x22, 0x78, 0x52,
    0xe6, 0xfe, 0x08, 0xe6, 0xff, 0xe4, 0xfc, 0xfd, 0x22, 0xe7, 0xc4, 0xf8, 0x54, 0xf0, 0xc8, 0x68,
    0xf7, 0x09, 0xe7, 0xc4, 0x54, 0x0f, 0x48, 0xf7, 0x22, 0xe6, 0xfc, 0xed, 0x75, 0xf0, 0x04, 0xa4,
    0x22, 0x12, 0x06, 0x7c, 0x8f, 0x48, 0x8e, 0x47, 0x8d, 0x46, 0x8c, 0x45, 0x22, 0xe0, 0xfe, 0xa3,
    0xe0, 0xfd, 0xee, 0xf6, 0xed, 0x08, 0xf6, 0x22, 0x13, 0xff, 0xc3, 0xe6, 0x9f, 0xff, 0x18, 0xe6,
    0x9e, 0xfe, 0x22, 0xe6, 0xc3, 0x13, 0xf7, 0x08, 0xe6, 0x13, 0x09, 0xf7, 0x22, 0xad, 0x39, 0xac,
    0x38, 0xfa, 0xf9, 0xf8, 0x12, 0x05, 0x28, 0x8f, 0x3b, 0x8e, 0x3a, 0x8d, 0x39, 0x8c, 0x38, 0xab,
    0x37, 0xaa, 0x36, 0xa9, 0x35, 0xa8, 0x34, 0x22, 0x93, 0xff, 0xe4, 0xfc, 0xfd, 0xfe, 0x12, 0x05,
    0x28, 0x8f, 0x37, 0x8e, 0x36, 0x8d, 0x35, 0x8c, 0x34, 0x22, 0x78, 0x84, 0xe6, 0xfe, 0x08, 0xe6,
    0xff, 0xe4, 0x8f, 0x37, 0x8e, 0x36, 0xf5, 0x35, 0xf5, 0x34, 0x22, 0x90, 0x0e, 0x8c, 0xe4, 0x93,
    0x25, 0xe0, 0x24, 0x0a, 0xf8, 0xe6, 0xfe, 0x08, 0xe6, 0xff, 0x22, 0xe6, 0xfe, 0x08, 0xe6, 0xff,
    0xe4, 0x8f, 0x3b, 0x8e, 0x3a, 0xf5, 0x39, 0xf5, 0x38, 0x22, 0x78, 0x4e, 0xe6, 0xfe, 0x08, 0xe6,
    0xff, 0x22, 0xef, 0x25, 0xe0, 0x24, 0x4e, 0xf8, 0xe6, 0xfc, 0x08, 0xe6, 0xfd, 0x22, 0x78, 0x89,
    0xef, 0x26, 0xf6, 0x18, 0xe4, 0x36, 0xf6, 0x22, 0x75, 0x89, 0x03, 0x75, 0xa8, 0x01, 0x75, 0xb8,
    0x04, 0x75, 0x34, 0xff, 0x75, 0x35, 0x0e, 0x75, 0x36, 0x15, 0x75, 0x37, 0x0d, 0x12, 0x0e, 0x9a,
    0x12, 0x00, 0x09, 0x12, 0x0f, 0x16, 0x12, 0x00, 0x06, 0xd2, 0x00, 0xd2, 0x34, 0xd2, 0xaf, 0x75,
    0x34, 0xff, 0x75, 0x35, 0x0e, 0x75, 0x36, 0x49, 0x75, 0x37, 0x03, 0x12, 0x0e, 0x9a, 0x30, 0x08,
    0x09, 0xc2, 0x34, 0x12, 0x08, 0xcb, 0xc2, 0x08, 0xd2, 0x34, 0x30, 0x0b, 0x09, 0xc2, 0x36, 0x12,
    0x02, 0x6c, 0xc2, 0x0b, 0xd2, 0x36, 0x30, 0x09, 0x09, 0xc2, 0x36, 0x12, 0x00, 0x0e, 0xc2, 0x09,
    0xd2, 0x36, 0x30, 0x0e, 0x03, 0x12, 0x06, 0xd7, 0x30, 0x35, 0xd3, 0x90, 0x30, 0x29, 0xe5, 0x1e,
    0xf0, 0xb4, 0x10, 0x05, 0x90, 0x30, 0x23, 0xe4, 0xf0, 0xc2, 0x35, 0x80, 0xc1, 0xe4, 0xf5, 0x4b,
    0x90, 0x0e, 0x7a, 0x93, 0xff, 0xe4, 0x8f, 0x37, 0xf5, 0x36, 0xf5, 0x35, 0xf5, 0x34, 0xaf, 0x37,
    0xae, 0x36, 0xad, 0x35, 0xac, 0x34, 0x90, 0x0e, 0x6a, 0x12, 0x0d, 0xf6, 0x8f, 0x37, 0x8e, 0x36,
    0x8d, 0x35, 0x8c, 0x34, 0x90, 0x0e, 0x72, 0x12, 0x06, 0x7c, 0xef, 0x45, 0x37, 0xf5, 0x37, 0xee,
    0x45, 0x36, 0xf5, 0x36, 0xed, 0x45, 0x35, 0xf5, 0x35, 0xec, 0x45, 0x34, 0xf5, 0x34, 0xe4, 0xf5,
    0x22, 0xf5, 0x23, 0x85, 0x37, 0x31, 0x85, 0x36, 0x30, 0x85, 0x35, 0x2f, 0x85, 0x34, 0x2e, 0x12,
    0x0f, 0x46, 0xe4, 0xf5, 0x22, 0xf5, 0x23, 0x90, 0x0e, 0x72, 0x12, 0x0d, 0xea, 0x12, 0x0f, 0x46,
    0xe4, 0xf5, 0x22, 0xf5, 0x23, 0x90, 0x0e, 0x6e, 0x12, 0x0d, 0xea, 0x02, 0x0f, 0x46, 0xe5, 0x40,
    0x24, 0xf2, 0xf5, 0x37, 0xe5, 0x3f, 0x34, 0x43, 0xf5, 0x36, 0xe5, 0x3e, 0x34, 0xa2, 0xf5, 0x35,
    0xe5, 0x3d, 0x34, 0x28, 0xf5, 0x34, 0xe5, 0x37, 0xff, 0xe4, 0xfe, 0xfd, 0xfc, 0x78, 0x18, 0x12,
    0x06, 0x69, 0x8f, 0x40, 0x8e, 0x3f, 0x8d, 0x3e, 0x8c, 0x3d, 0xe5, 0x37, 0x54, 0xa0, 0xff, 0xe5,
    0x36, 0xfe, 0xe4, 0xfd, 0xfc, 0x78, 0x07, 0x12, 0x06, 0x56, 0x78, 0x10, 0x12, 0x0f, 0x9a, 0xe4,
    0xff, 0xfe, 0xe5, 0x35, 0xfd, 0xe4, 0xfc, 0x78, 0x0e, 0x12, 0x06, 0x56, 0x12, 0x0f, 0x9d, 0xe4,
    0xff, 0xfe, 0xfd, 0xe5, 0x34, 0xfc, 0x78, 0x18, 0x12, 0x06, 0x56, 0x78, 0x08, 0x12, 0x0f, 0x9a,
    0x22, 0x8f, 0x3b, 0x8e, 0x3a, 0x8d, 0x39, 0x8c, 0x38, 0x22, 0x12, 0x06, 0x7c, 0x8f, 0x31, 0x8e,
    0x30, 0x8d, 0x2f, 0x8c, 0x2e, 0x22, 0x93, 0xf9, 0xf8, 0x02, 0x06, 0x69, 0x00, 0x00, 0x00, 0x00,
    0x12, 0x01, 0x17, 0x08, 0x31, 0x15, 0x53, 0x54, 0x44, 0x20, 0x20, 0x20, 0x20, 0x20, 0x13, 0x01,
    0x10, 0x01, 0x56, 0x40, 0x1a, 0x30, 0x29, 0x7e, 0x00, 0x30, 0x04, 0x20, 0xdf, 0x30, 0x05, 0x40,
    0xbf, 0x50, 0x03, 0x00, 0xfd, 0x50, 0x27, 0x01, 0xfe, 0x60, 0x00, 0x11, 0x00, 0x3f, 0x05, 0x30,
    0x00, 0x3f, 0x06, 0x22, 0x00, 0x3f, 0x01, 0x2a, 0x00, 0x3f, 0x02, 0x00, 0x00, 0x36, 0x06, 0x07,
    0x00, 0x3f, 0x0b, 0x0f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x30, 0x01, 0x40, 0xbf, 0x30, 0x01, 0x00,
    0xbf, 0x30, 0x29, 0x70, 0x00, 0x3a, 0x00, 0x00, 0xff, 0x3a, 0x00, 0x00, 0xff, 0x36, 0x03, 0x36,
    0x02, 0x41, 0x44, 0x58, 0x20, 0x18, 0x10, 0x0a, 0x04, 0x04, 0x00, 0x03, 0xff, 0x64, 0x00, 0x00,
    0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x04, 0x06, 0x06, 0x00, 0x03, 0x51, 0x00, 0x7a,
    0x50, 0x3c, 0x28, 0x1e, 0x10, 0x10, 0x50, 0x2d, 0x28, 0x16, 0x10, 0x10, 0x02, 0x00, 0x10, 0x0c,
    0x10, 0x04, 0x0c, 0x6e, 0x06, 0x05, 0x00, 0xa5, 0x5a, 0x00, 0xae, 0x35, 0xaf, 0x36, 0xe4, 0xfd,
    0xed, 0xc3, 0x95, 0x37, 0x50, 0x33, 0x12, 0x0f, 0xe2, 0xe4, 0x93, 0xf5, 0x38, 0x74, 0x01, 0x93,
    0xf5, 0x39, 0x45, 0x38, 0x60, 0x23, 0x85, 0x39, 0x82, 0x85, 0x38, 0x83, 0xe0, 0xfc, 0x12, 0x0f,
    0xe2, 0x74, 0x03, 0x93, 0x52, 0x04, 0x12, 0x0f, 0xe2, 0x74, 0x02, 0x93, 0x42, 0x04, 0x85, 0x39,
    0x82, 0x85, 0x38, 0x83, 0xec, 0xf0, 0x0d, 0x80, 0xc7, 0x22, 0x78, 0xbe, 0xe6, 0xd3, 0x08, 0xff,
    0xe6, 0x64, 0x80, 0xf8, 0xef, 0x64, 0x80, 0x98, 0x22, 0x93, 0xff, 0x7e, 0x00, 0xe6, 0xfc, 0x08,
    0xe6, 0xfd, 0x12, 0x04, 0xc1, 0x78, 0xc1, 0xe6, 0xfc, 0x08, 0xe6, 0xfd, 0xd3, 0xef, 0x9d, 0xee,
    0x9c, 0x22, 0x78, 0xbd, 0xd3, 0xe6, 0x64, 0x80, 0x94, 0x80, 0x22, 0x25, 0xe0, 0x24, 0x0a, 0xf8,
    0xe6, 0xfe, 0x08, 0xe6, 0xff, 0x22, 0xe5, 0x3c, 0xd3, 0x94, 0x00, 0x40, 0x0b, 0x90, 0x0e, 0x88,
    0x12, 0x0b, 0xf1, 0x90, 0x0e, 0x86, 0x80, 0x09, 0x90, 0x0e, 0x82, 0x12, 0x0b, 0xf1, 0x90, 0x0e,
    0x80, 0xe4, 0x93, 0xf5, 0x44, 0xa3, 0xe4, 0x93, 0xf5, 0x43, 0xd2, 0x06, 0x30, 0x06, 0x03, 0xd3,
    0x80, 0x01, 0xc3, 0x92, 0x0e, 0x22, 0xa2, 0xaf, 0x92, 0x32, 0xc2, 0xaf, 0xe5, 0x23, 0x45, 0x22,
    0x90, 0x0e, 0x5d, 0x60, 0x0e, 0x12, 0x0f, 0xcb, 0xe0, 0xf5, 0x2c, 0x12, 0x0f, 0xc8, 0xe0, 0xf5,
    0x2d, 0x80, 0x0c, 0x12, 0x0f, 0xcb, 0xe5, 0x30, 0xf0, 0x12, 0x0f, 0xc8, 0xe5, 0x31, 0xf0, 0xa2,
    0x32, 0x92, 0xaf, 0x22, 0xd2, 0x01, 0xc2, 0x02, 0xe4, 0xf5, 0x1f, 0xf5, 0x1e, 0xd2, 0x35, 0xd2,
    0x33, 0xd2, 0x36, 0xd2, 0x01, 0xc2, 0x02, 0xf5, 0x1f, 0xf5, 0x1e, 0xd2, 0x35, 0xd2, 0x33, 0x22,
    0xfb, 0xd3, 0xed, 0x9b, 0x74, 0x80, 0xf8, 0x6c, 0x98, 0x22, 0x12, 0x06, 0x69, 0xe5, 0x40, 0x2f,
    0xf5, 0x40, 0xe5, 0x3f, 0x3e, 0xf5, 0x3f, 0xe5, 0x3e, 0x3d, 0xf5, 0x3e, 0xe5, 0x3d, 0x3c, 0xf5,
    0x3d, 0x22, 0xc0, 0xe0, 0xc0, 0x83, 0xc0, 0x82, 0x90, 0x3f, 0x0d, 0xe0, 0xf5, 0x33, 0xe5, 0x33,
    0xf0, 0xd0, 0x82, 0xd0, 0x83, 0xd0, 0xe0, 0x32, 0x90, 0x0e, 0x5f, 0xe4, 0x93, 0xfe, 0x74, 0x01,
    0x93, 0xf5, 0x82, 0x8e, 0x83, 0x22, 0x78, 0x7f, 0xe4, 0xf6, 0xd8, 0xfd, 0x75, 0x81, 0xcd, 0x02,
    0x0c, 0x98, 0x8f, 0x82, 0x8e, 0x83, 0x75, 0xf0, 0x04, 0xed, 0x02, 0x06, 0xa5
};

static const uint8_t af_firmware_command_regs[][3] = {

    {0x30, 0x22, 0x03},
    {0x30, 0x23, 0x00},
    {0x30, 0x24, 0x00},
    {0x30, 0x25, 0x00},
    {0x30, 0x26, 0x00},
    {0x30, 0x27, 0x00},
    {0x30, 0x28, 0x00},
    {0x30, 0x29, 0x7f},

    {0x00, 0x00, 0x00}
};
#endif

#define NUM_BRIGHTNESS_LEVELS    (9)

#define NUM_CONTRAST_LEVELS      (7)
static const uint8_t contrast_regs[NUM_CONTRAST_LEVELS][1] = {
    {0x14}, /* -3 */
    {0x18}, /* -2 */
    {0x1C}, /* -1 */
    {0x00}, /* +0 */
    {0x10}, /* +1 */
    {0x18}, /* +2 */
    {0x1C}, /* +3 */
};

#define NUM_SATURATION_LEVELS    (7)
static const uint8_t saturation_regs[NUM_SATURATION_LEVELS][6] = {
    {0x0c, 0x30, 0x3d, 0x3e, 0x3d, 0x01}, /* -3 */
    {0x10, 0x3d, 0x4d, 0x4e, 0x4d, 0x01}, /* -2 */
    {0x15, 0x52, 0x66, 0x68, 0x66, 0x02}, /* -1 */
    {0x1a, 0x66, 0x80, 0x82, 0x80, 0x02}, /* +0 */
    {0x1f, 0x7a, 0x9a, 0x9c, 0x9a, 0x02}, /* +1 */
    {0x24, 0x8f, 0xb3, 0xb6, 0xb3, 0x03}, /* +2 */
    {0x2b, 0xab, 0xd6, 0xda, 0xd6, 0x04}, /* +3 */
};

static int reset(sensor_t *sensor) {
    int ret = 0;
    readout_x = 0;
    readout_y = 0;

    readout_w = ACTIVE_SENSOR_WIDTH;
    readout_h = ACTIVE_SENSOR_HEIGHT;

    hts_target = 0;

    // Reset all registers
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, SCCB_SYSTEM_CTRL_1, 0x11);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, SYSTEM_CTROL0, 0x82);

    // Delay 5 ms
    rt_thread_mdelay(5);

    // Write default registers
    for (int i = 0; default_regs[i][0]; i++) {
        int addr = (default_regs[i][0] << 8) | (default_regs[i][1] << 0);
        int data = default_regs[i][2];

        #if (OMV_OV5640_REV_Y_CHECK == 1)
        // Rev V (480 MHz / 20) -> 24 MHz PCLK / 3 * 100 = 800 MHz / 10 = 80 MHz PCLK.
        // Rev Y (400 MHz / 16) -> 25 MHz PCLK / 3 * 84 = 700 MHz / 10 = 70 MHz PCLK.
        if (HAL_GetREVID() < 0x2003) {
            // Is this REV Y?
            if (addr == SC_PLL_CONTRL2) {
                data = OMV_OV5640_REV_Y_CTRL2;
            }
            if (addr == SC_PLL_CONTRL3) {
                data = OMV_OV5640_REV_Y_CTRL3;
            }
        }
        #endif

        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, addr, data);
    }

    #if (OMV_ENABLE_OV5640_AF == 1)
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, SYSTEM_RESET_00, 0x20); // force mcu reset

    // Write firmware
    uint16_t fw_addr = __REV16(MCU_FIRMWARE_BASE);
    ret |= omv_i2c_write_bytes(&sensor->i2c_bus, sensor->slv_addr, (uint8_t *) &fw_addr, 2, OMV_I2C_XFER_SUSPEND);
    ret |= omv_i2c_write_bytes(&sensor->i2c_bus,
                               sensor->slv_addr,
                               (uint8_t *) af_firmware_regs,
                               sizeof(af_firmware_regs),
                               OMV_I2C_XFER_NO_FLAGS);

    for (int i = 0; af_firmware_command_regs[i][0]; i++) {
        ret |=
            omv_i2c_writeb2(&sensor->i2c_bus,
                            sensor->slv_addr,
                            (af_firmware_command_regs[i][0] << 8) | (af_firmware_command_regs[i][1] << 0),
                            af_firmware_command_regs[i][2]);
    }

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, SYSTEM_RESET_00, 0x00); // release mcu reset
    #endif

    // Delay 300 ms
    if (!sensor->disable_delays) {
        rt_thread_mdelay(300);
    }

    return ret;
}

static int sleep(sensor_t *sensor, int enable) {
    uint8_t reg;
    if (enable) {
        reg = 0x42;
    } else {
        reg = 0x02;
    }

    return omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, SYSTEM_CTROL0, reg);
}

static int read_reg(sensor_t *sensor, uint16_t reg_addr) {
    uint8_t reg_data;
    if (omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, reg_addr, &reg_data) != 0) {
        return -1;
    }
    return reg_data;
}

static int write_reg(sensor_t *sensor, uint16_t reg_addr, uint16_t reg_data) {
    return omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, reg_addr, reg_data);
}

// HTS (Horizontal Time) is the readout width plus the HSYNC_TIME time. However, if this value gets
// too low the OV5640 will crash. The minimum was determined empirically with testing...
// Additionally, when the image width gets too large we need to slow down the line transfer rate by
// increasing HTS so that DCMI_DMAConvCpltUser() can keep up with the data rate.
//
// WARNING! IF YOU CHANGE ANYTHING HERE RETEST WITH **ALL** RESOLUTIONS FOR THE AFFECTED MODE!
static int calculate_hts(sensor_t *sensor, uint16_t width) {
    uint16_t hts = hts_target;

    if ((sensor->pixformat == PIXFORMAT_GRAYSCALE) || (sensor->pixformat == PIXFORMAT_BAYER) ||
        (sensor->pixformat == PIXFORMAT_JPEG)) {
        if (width <= 1280) {
            hts = IM_MAX((width * 2) + 8, hts_target);
        }
    } else {
        if (width > 640) {
            hts = IM_MAX((width * 2) + 8, hts_target);
        }
    }

    if (width <= 640) {
        hts += 160;               // Fix image quality at low resolutions.

    }
    return IM_MAX(hts + HSYNC_TIME, (SENSOR_WIDTH + HSYNC_TIME) / 2); // Fix to prevent crashing.
}

// VTS (Vertical Time) is the readout height plus the VYSNC_TIME time. However, if this value gets
// too low the OV5640 will crash. The minimum was determined empirically with testing...
//
// WARNING! IF YOU CHANGE ANYTHING HERE RETEST WITH **ALL** RESOLUTIONS FOR THE AFFECTED MODE!
static int calculate_vts(sensor_t *sensor, uint16_t readout_height) {
    return IM_MAX(readout_height + VYSNC_TIME, (SENSOR_HEIGHT + VYSNC_TIME) / 8); // Fix to prevent crashing.
}

static int set_pixformat(sensor_t *sensor, pixformat_t pixformat) {
    uint8_t reg;
    int ret = 0;

    // Not a multiple of 8. The JPEG encoder on the OV5640 can't handle this.
    if ((pixformat == PIXFORMAT_JPEG) && ((resolution[sensor->framesize][0] % 8) || (resolution[sensor->framesize][1] % 8))) {
        return -1;
    }

    // Readout speed too fast. The DCMI_DMAConvCpltUser() line callback overhead is too much to handle the line transfer speed.
    // If we were to slow the pixclk down these resolutions would work. As of right now, the image shakes and scrolls with
    // the current line transfer speed. Note that there's an overhead to the DCMI_DMAConvCpltUser() function. It's not the
    // memory copy operation that's too slow. It's that there's too much overhead in the DCMI_DMAConvCpltUser() method
    // to even have time to start the line transfer. If it were possible to slow the line readout speed of the OV5640
    // this would enable these resolutions below. However, there's nothing in the datasheet that when modified does this.
    if (((pixformat == PIXFORMAT_GRAYSCALE) || (pixformat == PIXFORMAT_BAYER) || (pixformat == PIXFORMAT_JPEG))
        && ((sensor->framesize == FRAMESIZE_QQCIF)
            || (sensor->framesize == FRAMESIZE_QQSIF)
            || (sensor->framesize == FRAMESIZE_HQQQVGA)
            || (sensor->framesize == FRAMESIZE_HQQVGA))) {
        return -1;
    }

    switch (pixformat) {
//        case PIXFORMAT_GRAYSCALE:
//            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, FORMAT_CONTROL, 0x10);
//            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, FORMAT_CONTROL_MUX, 0x00);
//            break;
        case PIXFORMAT_RGB565:
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, FORMAT_CONTROL, 0x6F);
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, FORMAT_CONTROL_MUX, 0x01);
            break;
        case PIXFORMAT_GRAYSCALE:
        case PIXFORMAT_YUV422:
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, FORMAT_CONTROL, 0x30);
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, FORMAT_CONTROL_MUX, 0x00);
            break;
        case PIXFORMAT_BAYER:
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, FORMAT_CONTROL, 0x00);
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, FORMAT_CONTROL_MUX, 0x01);
            break;
        case PIXFORMAT_JPEG:
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, FORMAT_CONTROL, 0x30);
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, FORMAT_CONTROL_MUX, 0x00);
            break;
        default:
            return -1;
    }

    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_TC_REG_21, &reg);
    ret |=
        omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_TC_REG_21,
                        (reg & 0xDF) | ((pixformat == PIXFORMAT_JPEG) ? 0x20 : 0x00));

    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, SYSTEM_RESET_02, &reg);
    ret |=
        omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, SYSTEM_RESET_02,
                        (reg & 0xE3) | ((pixformat == PIXFORMAT_JPEG) ? 0x00 : 0x1C));

    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, CLOCK_ENABLE_02, &reg);
    ret |=
        omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, CLOCK_ENABLE_02,
                        (reg & 0xD7) | ((pixformat == PIXFORMAT_JPEG) ? 0x28 : 0x00));

    if (hts_target) {
        uint16_t sensor_hts = calculate_hts(sensor, resolution[sensor->framesize][0]);

        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_HTS_H, sensor_hts >> 8);
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_HTS_L, sensor_hts);
    }

    return ret;
}

static int set_framesize(sensor_t *sensor, framesize_t framesize) {
    uint8_t reg;
    int ret = 0;
    uint16_t w = resolution[framesize][0];
    uint16_t h = resolution[framesize][1];

    // Not a multiple of 8. The JPEG encoder on the OV5640 can't handle this.
    if ((sensor->pixformat == PIXFORMAT_JPEG) && ((w % 8) || (h % 8))) {
        return -1;
    }

    // Readout speed too fast. The DCMI_DMAConvCpltUser() line callback overhead is too much to handle the line transfer speed.
    // If we were to slow the pixclk down these resolutions would work. As of right now, the image shakes and scrolls with
    // the current line transfer speed. Note that there's an overhead to the DCMI_DMAConvCpltUser() function. It's not the
    // memory copy operation that's too slow. It's that there's too much overhead in the DCMI_DMAConvCpltUser() method
    // to even have time to start the line transfer. If it were possible to slow the line readout speed of the OV5640
    // this would enable these resolutions below. However, there's nothing in the datasheet that when modified does this.
    if (((sensor->pixformat == PIXFORMAT_GRAYSCALE) || (sensor->pixformat == PIXFORMAT_BAYER) ||
         (sensor->pixformat == PIXFORMAT_JPEG))
        && ((framesize == FRAMESIZE_QQCIF)
            || (framesize == FRAMESIZE_QQSIF)
            || (framesize == FRAMESIZE_HQQQVGA)
            || (framesize == FRAMESIZE_HQQVGA))) {
        return -1;
    }

    // Generally doesn't work for anything.
    if (framesize == FRAMESIZE_QQQQVGA) {
        return -1;
    }

    // Invalid resolution.
    if ((w > ACTIVE_SENSOR_WIDTH) || (h > ACTIVE_SENSOR_HEIGHT)) {
        return -1;
    }

    // Step 0: Clamp readout settings.

    readout_w = IM_MAX(readout_w, w);
    readout_h = IM_MAX(readout_h, h);

    int readout_x_max = (ACTIVE_SENSOR_WIDTH - readout_w) / 2;
    int readout_y_max = (ACTIVE_SENSOR_HEIGHT - readout_h) / 2;
    readout_x = IM_MAX(IM_MIN(readout_x, readout_x_max), -readout_x_max);
    readout_y = IM_MAX(IM_MIN(readout_y, readout_y_max), -readout_y_max);

    // Step 1: Determine readout area and subsampling amount.

    uint16_t sensor_div = 0;

    if ((w > (readout_w / 2)) || (h > (readout_h / 2))) {
        sensor_div = 1;
    } else {
        sensor_div = 2;
    }

    // Step 2: Determine horizontal and vertical start and end points.

    uint16_t sensor_w = readout_w + DUMMY_WIDTH_BUFFER; // camera hardware needs dummy pixels to sync
    uint16_t sensor_h = readout_h + DUMMY_HEIGHT_BUFFER; // camera hardware needs dummy lines to sync

    uint16_t sensor_ws =
        IM_MAX(IM_MIN((((ACTIVE_SENSOR_WIDTH - sensor_w) / 4) + (readout_x / 2)) * 2, ACTIVE_SENSOR_WIDTH - sensor_w),
               -(DUMMY_WIDTH_BUFFER / 2)) + DUMMY_COLUMNS;                                                                                                                          // must be multiple of 2
    uint16_t sensor_we = sensor_ws + sensor_w - 1;

    uint16_t sensor_hs =
        IM_MAX(IM_MIN((((ACTIVE_SENSOR_HEIGHT - sensor_h) / 4) - (readout_y / 2)) * 2, ACTIVE_SENSOR_HEIGHT - sensor_h),
               -(DUMMY_HEIGHT_BUFFER / 2)) + DUMMY_LINES;                                                                                                                            // must be multiple of 2
    uint16_t sensor_he = sensor_hs + sensor_h - 1;

    // Step 3: Determine scaling window offset.

    float ratio = IM_MIN((readout_w / sensor_div) / ((float) w), (readout_h / sensor_div) / ((float) h));

    uint16_t w_mul = w * ratio;
    uint16_t h_mul = h * ratio;
    uint16_t x_off = ((sensor_w / sensor_div) - w_mul) / 2;
    uint16_t y_off = ((sensor_h / sensor_div) - h_mul) / 2;

    // Step 4: Compute total frame time.

    hts_target = sensor_w / sensor_div;

    uint16_t sensor_hts = calculate_hts(sensor, w);
    uint16_t sensor_vts = calculate_vts(sensor, sensor_h / sensor_div);

    uint16_t sensor_x_inc = (((sensor_div * 2) - 1) << 4) | (1 << 0); // odd[7:4]/even[3:0] pixel inc on the bayer pattern
    uint16_t sensor_y_inc = (((sensor_div * 2) - 1) << 4) | (1 << 0); // odd[7:4]/even[3:0] pixel inc on the bayer pattern

    // Step 5: Write regs.

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_HS_H, sensor_ws >> 8);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_HS_L, sensor_ws);

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_VS_H, sensor_hs >> 8);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_VS_L, sensor_hs);

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_HW_H, sensor_we >> 8);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_HW_L, sensor_we);

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_VH_H, sensor_he >> 8);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_VH_L, sensor_he);

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_DVPHO_H, w >> 8);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_DVPHO_L, w);

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_DVPVO_H, h >> 8);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_DVPVO_L, h);

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_HTS_H, sensor_hts >> 8);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_HTS_L, sensor_hts);

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_VTS_H, sensor_vts >> 8);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_VTS_L, sensor_vts);

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_HOFFSET_H, x_off >> 8);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_HOFFSET_L, x_off);

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_VOFFSET_H, y_off >> 8);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_VOFFSET_L, y_off);

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_X_INC, sensor_x_inc);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_Y_INC, sensor_y_inc);

    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_TC_REG_20, &reg);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_TC_REG_20, (reg & 0xFE) | (sensor_div > 1));

    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_TC_REG_21, &reg);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_TC_REG_21, (reg & 0xFE) | (sensor_div > 1));

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, VFIFO_HSIZE_H, w >> 8);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, VFIFO_HSIZE_L, w);

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, VFIFO_VSIZE_H, h >> 8);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, VFIFO_VSIZE_L, h);

    return ret;
}

static int set_contrast(sensor_t *sensor, int level) {
    int ret = 0;

    int new_level = level + (NUM_CONTRAST_LEVELS / 2);
    if (new_level < 0 || new_level >= NUM_CONTRAST_LEVELS) {
        return -1;
    }

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x3212, 0x03); // start group 3
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5586, (new_level + 5) << 2);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5585, contrast_regs[new_level][0]);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x3212, 0x13); // end group 3
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x3212, 0xa3); // launch group 3

    return ret;
}

static int set_brightness(sensor_t *sensor, int level) {
    int ret = 0;

    int new_level = level + (NUM_BRIGHTNESS_LEVELS / 2);
    if (new_level < 0 || new_level >= NUM_BRIGHTNESS_LEVELS) {
        return -1;
    }

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x3212, 0x03); // start group 3
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5587, abs(level) << 4);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5588, (level < 0) ? 0x09 : 0x01);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x3212, 0x13); // end group 3
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x3212, 0xa3); // launch group 3

    return ret;
}

static int set_saturation(sensor_t *sensor, int level) {
    int ret = 0;

    int new_level = level + (NUM_SATURATION_LEVELS / 2);
    if (new_level < 0 || new_level >= NUM_SATURATION_LEVELS) {
        return -1;
    }

    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x3212, 0x03); // start group 3
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5581, 0x1c);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5582, 0x5a);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5583, 0x06);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5584, saturation_regs[new_level][0]);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5585, saturation_regs[new_level][1]);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5586, saturation_regs[new_level][2]);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5587, saturation_regs[new_level][3]);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5588, saturation_regs[new_level][4]);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5589, saturation_regs[new_level][5]);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x558b, 0x98);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x558a, 0x01);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x3212, 0x13); // end group 3
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x3212, 0xa3); // launch group 3

    return ret;
}

static int set_gainceiling(sensor_t *sensor, gainceiling_t gainceiling) {
    uint8_t reg;
    int ret = 0;

    int new_gainceiling = 16 << (gainceiling + 1);
    if (new_gainceiling >= 1024) {
        return -1;
    }

    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AEC_GAIN_CEILING_H, &reg);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AEC_GAIN_CEILING_H, (reg & 0xFC) | (new_gainceiling >> 8));
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AEC_GAIN_CEILING_L, new_gainceiling);

    return ret;
}

static int set_quality(sensor_t *sensor, int qs) {
    uint8_t reg;
    int ret = omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, JPEG_CTRL07, &reg);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, JPEG_CTRL07, (reg & 0xC0) | (qs >> 2));

    return ret;
}

static int set_colorbar(sensor_t *sensor, int enable) {
    uint8_t reg;
    int ret = omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, PRE_ISP_TEST, &reg);
    return omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, PRE_ISP_TEST, (reg & 0x7F) | (enable ? 0x80 : 0x00)) | ret;
}

static int set_auto_gain(sensor_t *sensor, int enable, float gain_db, float gain_db_ceiling) {
    uint8_t reg;
    int ret = omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AEC_PK_MANUAL, &reg);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AEC_PK_MANUAL, (reg & 0xFD) | ((enable == 0) << 1));

    if ((enable == 0) && (!isnanf(gain_db)) && (!isinff(gain_db))) {
        int gain = IM_MAX(IM_MIN(fast_roundf(expf((gain_db / 20.0f) * M_LN10) * 16.0f), 1023), 0);

        ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AEC_PK_REAL_GAIN_H, &reg);
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AEC_PK_REAL_GAIN_H, (reg & 0xFC) | (gain >> 8));
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AEC_PK_REAL_GAIN_L, gain);
    } else if ((enable != 0) && (!isnanf(gain_db_ceiling)) && (!isinff(gain_db_ceiling))) {
        int gain_ceiling = IM_MAX(IM_MIN(fast_roundf(expf((gain_db_ceiling / 20.0f) * M_LN10) * 16.0f), 1023), 0);

        ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AEC_GAIN_CEILING_H, &reg);
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AEC_GAIN_CEILING_H, (reg & 0xFC) | (gain_ceiling >> 8));
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AEC_GAIN_CEILING_L, gain_ceiling);
    }

    return ret;
}

static int get_gain_db(sensor_t *sensor, float *gain_db) {
    uint8_t gainh, gainl;

    int ret = omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AEC_PK_REAL_GAIN_H, &gainh);
    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AEC_PK_REAL_GAIN_L, &gainl);

    *gain_db = 20.0f * log10f((((gainh & 0x3) << 8) | gainl) / 16.0f);

    return ret;
}

static int calc_pclk_freq(uint8_t sc_pll_ctrl_0,
                          uint8_t sc_pll_ctrl_1,
                          uint8_t sc_pll_ctrl_2,
                          uint8_t sc_pll_ctrl_3,
                          uint8_t sys_root_div) {
    uint32_t pclk_freq = sensor_get_xclk_frequency();
    pclk_freq /= ((sc_pll_ctrl_3 & 0x10) != 0x00) ? 2 : 1;
    pclk_freq /= ((sc_pll_ctrl_0 & 0x0F) == 0x0A) ? 10 : 8;
    switch (sc_pll_ctrl_3 & 0x0F) {
        case  0: pclk_freq /= 1; break;
        case  1: pclk_freq /= 2; break;
        case  2: pclk_freq /= 3; break;
        case  3: pclk_freq /= 4; break;
        case  4: pclk_freq /= 6; break;
        case  5: pclk_freq /= 8; break;
        default: pclk_freq /= 3; break;
    }
    pclk_freq *= sc_pll_ctrl_2;
    sc_pll_ctrl_1 >>= 4;
    pclk_freq /= sc_pll_ctrl_1;
    switch (sys_root_div & 0x30) {
        case 0x00: pclk_freq /= 1; break;
        case 0x10: pclk_freq /= 2; break;
        case 0x20: pclk_freq /= 4; break;
        case 0x30: pclk_freq /= 8; break;
        default:   pclk_freq /= 1; break;
    }
    return (int) pclk_freq;
}

static int set_auto_exposure(sensor_t *sensor, int enable, int exposure_us) {
    uint8_t reg, spc0, spc1, spc2, spc3, sysrootdiv, hts_h, hts_l, vts_h, vts_l;
    int ret = omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AEC_PK_MANUAL, &reg);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AEC_PK_MANUAL, (reg & 0xFE) | ((enable == 0) << 0));

    if ((enable == 0) && (exposure_us >= 0)) {
        ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, SC_PLL_CONTRL0, &spc0);
        ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, SC_PLL_CONTRL1, &spc1);
        ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, SC_PLL_CONTRL2, &spc2);
        ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, SC_PLL_CONTRL3, &spc3);
        ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, SYSTEM_ROOT_DIVIDER, &sysrootdiv);

        ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_HTS_H, &hts_h);
        ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_HTS_L, &hts_l);

        ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_VTS_H, &vts_h);
        ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_VTS_L, &vts_l);

        uint16_t hts = (hts_h << 8) | hts_l;
        uint16_t vts = (vts_h << 8) | vts_l;

        int pclk_freq = calc_pclk_freq(spc0, spc1, spc2, spc3, sysrootdiv);
        int clocks_per_us = pclk_freq / 1000000;
        int exposure = IM_MAX(IM_MIN((exposure_us * clocks_per_us) / hts, 0xFFFF), 0x0000);

        int new_vts = IM_MAX(exposure, vts);

        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AEC_PK_EXPOSURE_0, exposure >> 12);
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AEC_PK_EXPOSURE_1, exposure >> 4);
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AEC_PK_EXPOSURE_2, exposure << 4);

        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_VTS_H, new_vts >> 8);
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_VTS_L, new_vts);
    }

    return ret;
}

static int get_exposure_us(sensor_t *sensor, int *exposure_us) {
    uint8_t spc0, spc1, spc2, spc3, sysrootdiv, aec_0, aec_1, aec_2, hts_h, hts_l, vts_h, vts_l;
    int ret = 0;

    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, SC_PLL_CONTRL0, &spc0);
    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, SC_PLL_CONTRL1, &spc1);
    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, SC_PLL_CONTRL2, &spc2);
    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, SC_PLL_CONTRL3, &spc3);
    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, SYSTEM_ROOT_DIVIDER, &sysrootdiv);

    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AEC_PK_EXPOSURE_0, &aec_0);
    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AEC_PK_EXPOSURE_1, &aec_1);
    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AEC_PK_EXPOSURE_2, &aec_2);

    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_HTS_H, &hts_h);
    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_HTS_L, &hts_l);

    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_VTS_H, &vts_h);
    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_VTS_L, &vts_l);

    uint32_t aec = ((aec_0 << 16) | (aec_1 << 8) | aec_2) >> 4;
    uint16_t hts = (hts_h << 8) | hts_l;
    uint16_t vts = (vts_h << 8) | vts_l;

    aec = IM_MIN(aec, vts);

    int pclk_freq = calc_pclk_freq(spc0, spc1, spc2, spc3, sysrootdiv);
    int clocks_per_us = pclk_freq / 1000000;
    *exposure_us = (aec * hts) / clocks_per_us;

    return ret;
}

static int set_auto_whitebal(sensor_t *sensor, int enable, float r_gain_db, float g_gain_db, float b_gain_db) {
    uint8_t reg;
    int ret = omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AWB_MANUAL_CONTROL, &reg);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AWB_MANUAL_CONTROL, (reg & 0xFE) | (enable == 0));

    if ((enable == 0) && (!isnanf(r_gain_db)) && (!isnanf(g_gain_db)) && (!isnanf(b_gain_db))
        && (!isinff(r_gain_db)) && (!isinff(g_gain_db)) && (!isinff(b_gain_db))) {

        int r_gain = IM_MAX(IM_MIN(fast_roundf(expf((r_gain_db / 20.0f) * M_LN10)), 4095), 0);
        int g_gain = IM_MAX(IM_MIN(fast_roundf(expf((g_gain_db / 20.0f) * M_LN10)), 4095), 0);
        int b_gain = IM_MAX(IM_MIN(fast_roundf(expf((b_gain_db / 20.0f) * M_LN10)), 4095), 0);

        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AWB_R_GAIN_H, r_gain >> 8);
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AWB_R_GAIN_L, r_gain);
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AWB_G_GAIN_H, g_gain >> 8);
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AWB_G_GAIN_L, g_gain);
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AWB_B_GAIN_H, b_gain >> 8);
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AWB_B_GAIN_L, b_gain);
    }

    return ret;
}

static int get_rgb_gain_db(sensor_t *sensor, float *r_gain_db, float *g_gain_db, float *b_gain_db) {
    uint8_t redh, redl, greenh, greenl, blueh, bluel;

    int ret = omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AWB_R_GAIN_H, &redh);
    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AWB_R_GAIN_L, &redl);
    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AWB_G_GAIN_H, &greenh);
    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AWB_G_GAIN_L, &greenl);
    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AWB_B_GAIN_H, &blueh);
    ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AWB_B_GAIN_L, &bluel);

    *r_gain_db = 20.0f * log10f(((redh & 0xF) << 8) | redl);
    *g_gain_db = 20.0f * log10f(((greenh & 0xF) << 8) | greenl);
    *b_gain_db = 20.0f * log10f(((blueh & 0xF) << 8) | bluel);

    return ret;
}

static int set_auto_blc(sensor_t *sensor, int enable, int *regs) {
    uint8_t reg;
    int ret = omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, BLC_CTRL_00, &reg);
    ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, BLC_CTRL_00, (reg & 0xFE) | (enable != 0));

    if ((enable == 0) && (regs != NULL)) {
        for (uint32_t i = 0; i < sensor->hw_flags.blc_size; i++) {
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, BLACK_LEVEL_00_H + i, regs[i]);
        }
    }

    return ret;
}

static int get_blc_regs(sensor_t *sensor, int *regs) {
    int ret = 0;

    for (uint32_t i = 0; i < sensor->hw_flags.blc_size; i++) {
        uint8_t reg;
        ret |= omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, BLACK_LEVEL_00_H + i, &reg);
        regs[i] = reg;
    }

    return ret;
}

static int set_hmirror(sensor_t *sensor, int enable) {
    uint8_t reg;
    int ret = omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_TC_REG_21, &reg);
    if (enable) {
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_TC_REG_21, reg | 0x06);
    } else {
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_TC_REG_21, reg & 0xF9);
    }
    return ret;
}

static int set_vflip(sensor_t *sensor, int enable) {
    uint8_t reg;
    int ret = omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_TC_REG_20, &reg);
    if (!enable) {
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_TC_REG_20, reg | 0x06);
    } else {
        ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, TIMING_TC_REG_20, reg & 0xF9);
    }
    return ret;
}

static int set_special_effect(sensor_t *sensor, sde_t sde) {
    int ret = 0;

    switch (sde) {
        case SDE_NEGATIVE:
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x3212, 0x03); // start group 3
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5580, 0x40);
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5003, 0x08);
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5583, 0x40); // sat U
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5584, 0x10); // sat V
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x3212, 0x13); // end group 3
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x3212, 0xa3); // latch group 3
            break;
        case SDE_NORMAL:
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x3212, 0x03); // start group 3
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5580, 0x06);
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5583, 0x40); // sat U
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5584, 0x10); // sat V
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x5003, 0x08);
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x3212, 0x13); // end group 3
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, 0x3212, 0xa3); // latch group 3
            break;
        default:
            return -1;
    }

    return ret;
}

static int set_lens_correction(sensor_t *sensor, int enable, int radi, int coef) {
    uint8_t reg;
    int ret = omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, ISP_CONTROL_00, &reg);
    return omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, ISP_CONTROL_00, (reg & 0x7F) | (enable ? 0x80 : 0x00)) | ret;
}

static int ioctl(sensor_t *sensor, int request, va_list ap) {
    int ret = 0;
    uint8_t reg;

    switch (request) {
        case IOCTL_SET_READOUT_WINDOW: {
            int tmp_readout_x = va_arg(ap, int);
            int tmp_readout_y = va_arg(ap, int);
            int tmp_readout_w = IM_MAX(IM_MIN(va_arg(ap, int), ACTIVE_SENSOR_WIDTH), resolution[sensor->framesize][0]);
            int tmp_readout_h = IM_MAX(IM_MIN(va_arg(ap, int), ACTIVE_SENSOR_HEIGHT), resolution[sensor->framesize][1]);
            int readout_x_max = (ACTIVE_SENSOR_WIDTH - tmp_readout_w) / 2;
            int readout_y_max = (ACTIVE_SENSOR_HEIGHT - tmp_readout_h) / 2;
            tmp_readout_x = IM_MAX(IM_MIN(tmp_readout_x, readout_x_max), -readout_x_max);
            tmp_readout_y = IM_MAX(IM_MIN(tmp_readout_y, readout_y_max), -readout_y_max);
            bool changed = (tmp_readout_x != readout_x) || (tmp_readout_y != readout_y) || (tmp_readout_w != readout_w) ||
                           (tmp_readout_h != readout_h);
            readout_x = tmp_readout_x;
            readout_y = tmp_readout_y;
            readout_w = tmp_readout_w;
            readout_h = tmp_readout_h;
            if (changed && (sensor->framesize != FRAMESIZE_INVALID)) {
                set_framesize(sensor, sensor->framesize);
            }
            break;
        }
        case IOCTL_GET_READOUT_WINDOW: {
            *va_arg(ap, int *) = readout_x;
            *va_arg(ap, int *) = readout_y;
            *va_arg(ap, int *) = readout_w;
            *va_arg(ap, int *) = readout_h;
            break;
        }
    #if (OMV_ENABLE_OV5640_AF == 1)
        case IOCTL_TRIGGER_AUTO_FOCUS: {
            ret = omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AF_CMD_MAIN, 0x03);
            break;
        }
        case IOCTL_PAUSE_AUTO_FOCUS: {
            ret = omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AF_CMD_MAIN, 0x06);
            break;
        }
        case IOCTL_RESET_AUTO_FOCUS: {
            ret = omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AF_CMD_MAIN, 0x08);
            break;
        }
        case IOCTL_WAIT_ON_AUTO_FOCUS: {
            uint32_t start_tick = rt_tick_get(), delay_ms = va_arg(ap, uint32_t);
            for (;;) {
                ret = omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AF_CMD_ACK, &reg);
                if ((ret < 0) || (!reg)) {
                    break;
                }
                if ((rt_tick_get() - start_tick) >= delay_ms) {
                    return -1;
                }
                rt_thread_mdelay(1);
            }
            break;
        }
    #endif
        case IOCTL_SET_NIGHT_MODE: {
            int enable = va_arg(ap, int);
            ret = omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AEC_CTRL_00, &reg);
            ret |= omv_i2c_writeb2(&sensor->i2c_bus, sensor->slv_addr, AEC_CTRL_00,
                                   (reg & 0xFB) | ((enable != 0) << 2));
            break;
        }
        case IOCTL_GET_NIGHT_MODE: {
            int *enable = va_arg(ap, int *);
            ret = omv_i2c_readb2(&sensor->i2c_bus, sensor->slv_addr, AEC_CTRL_00, &reg);
            if (ret >= 0) {
                *enable = reg & 0x4;
            }
            break;
        }
        default: {
            ret = -1;
            break;
        }
    }

    return ret;
}

int ov5640_init(sensor_t *sensor) {
    // Initialize sensor structure.
    sensor->reset = reset;
    sensor->sleep = sleep;
    sensor->read_reg = read_reg;
    sensor->write_reg = write_reg;
    sensor->set_pixformat = set_pixformat;
    sensor->set_framesize = set_framesize;
    sensor->set_contrast = set_contrast;
    sensor->set_brightness = set_brightness;
    sensor->set_saturation = set_saturation;
    sensor->set_gainceiling = set_gainceiling;
    sensor->set_quality = set_quality;
    sensor->set_colorbar = set_colorbar;
    sensor->set_auto_gain = set_auto_gain;
    sensor->get_gain_db = get_gain_db;
    sensor->set_auto_exposure = set_auto_exposure;
    sensor->get_exposure_us = get_exposure_us;
    sensor->set_auto_whitebal = set_auto_whitebal;
    sensor->get_rgb_gain_db = get_rgb_gain_db;
    sensor->set_auto_blc = set_auto_blc;
    sensor->get_blc_regs = get_blc_regs;
    sensor->set_hmirror = set_hmirror;
    sensor->set_vflip = set_vflip;
    sensor->set_special_effect = set_special_effect;
    sensor->set_lens_correction = set_lens_correction;
    sensor->ioctl = ioctl;

    // Set sensor flags
    sensor->hw_flags.vsync = 1;
    sensor->hw_flags.hsync = 0;
    sensor->hw_flags.pixck = 1;
    sensor->hw_flags.fsync = 0;
    sensor->hw_flags.jpege = 1;
    sensor->hw_flags.jpeg_mode = 4;
    sensor->hw_flags.gs_bpp = 2;
    sensor->hw_flags.rgb_swap = 0;
    sensor->hw_flags.yuv_order = SENSOR_HW_FLAGS_YVU422;
    sensor->hw_flags.blc_size = 8;

    return 0;
}
#endif // (OMV_ENABLE_OV5640 == 1)
